Method for simultaneous localization and mapping

ABSTRACT

The present disclosure discloses a method for simultaneous localization and mapping, which can reliably handle strong rotation and fast motion. The method provided a simultaneous localization and mapping algorithm framework based on a key frame, which can support rapid local map extension. Under this framework, a new feature tracking method based on multiple homography matrices is provided, and this method is efficient and robust under strong rotation and fast motion. A camera orientation optimization framework based on a sliding window is further provided to increase motion constraint between successive frames with simulated or actual IMU data. Finally, a method for obtaining a real scale of a specific plane and scene is provided in such a manner that a virtual object is placed on a specific plane in real size.

TECHNICAL FIELD

The present disclosure relates to a monocular camera tracking method which can be applied to augmented reality, in particular to a robust camera tracking method based on a key frame.

BACKGROUND

In recent years, with the popularity of mobile terminals and wearable devices, augmented reality has gained unprecedented attention. Realistic augmented reality requires accurately estimating the camera orientation and reconstructing the scene in real time, which is also the problem to be solved by the simultaneous localization and mapping (SLAM) technology.

Due to the complexity of the real scene, the traditional SLAM method is not effective in practical applications. For example, when a new user uses a mobile device to shoot a scene and view an augmented reality effect, the user often moves and rotates the mobile device casually. Therefore, strong rotation and fast motion are very common. However, even the most advanced SLAM system can hardly handle such complex situations.

A good augmented reality experience requires that the SLAM system can handle a wide variety of complex camera motions while making it easy for the novices to get started. Moreover, it is necessary to ensure that the frequency lost in the camera is as little as possible under circumstances of rapid motion and severe motion blur. Even in the case of tracking failures, the camera should be relocalized quickly to avoid long waits.

Visual SLAM can be divided into two categories: filter-based methods and key frame-based methods. MonoSLAM (A. J. Davison, I. D. Reid, N. D. Molton, and O. Stasse. MonoSLAM: Real-time single camera SLAM. IEEE Transactions on Pattern Analysis and Machine Intelligence, 29 (6): 1052-1067, 2007) is a typical filter-based Method. The parameter of the camera motion and the scene three-dimensional point position are collectively represented as a high-dimensional probability state. An Extended Kalman Filter (EKF) is used to predict and update the state distribution of each frame. The biggest disadvantage of the MonoSLAM method is that the time complexity is O (N3) with N being the number of landmarks, so it is only suitable for a small scene of several hundreds of points. In addition, it is easy for the EKF to accumulate linearity errors. In order to overcome the above limitations, PTAM (G. Klein and D. W. Murray. Parallel tracking and mapping for small AR workspaces. In 6th IEEE/ACM International Symposium on Mixed and Augmented Reality, pages 225-234, 2007) adopts a novel parallel tracking and mapping framework based on key frames. In order to achieve real-time performance, the tracking is independent from the mapping. In order to ensure high precision, the background thread performs bundle adjustment (BA) on all key frames and the scene three-dimensional points. As summarized in (H. Strasdat, J. M. Montiel, and A. J. Davison. Visual SLAM: why filter? Image and Vision Computing, 30 (2):65-77, 2012), the method based on bundle adjustment of key frames is superior to the filter-based method, especially when the number N of landmarks is very large.

Recently, many of the most advanced SLAM systems have adopted key frame-based frameworks, such as RDSLAM (W. Tan, H. Liu, Z. Dong, G. Zhang, and H. Bao. Robust monocular SLAM in dynamic environments. In IEEE International Symposium on Mixed and Augmented Reality, pages 209 -218, 2013) and ORB-SLAM (R. Mur-Artal, J. Montiel, and J. D. Tardos. ORB-SLAM: a versatile and accurate monocular SLAM system. IEEE Transactions on Robotics, 31 (5):1147-1163, 2015). RDSLAM proposes a modified RANSAC method to achieve real-time localization in dynamic scenes by detecting the appearance and structural changes of the scene. ORB-SLAM adopts ORB features for camera localization, mapping, relocating, and loop detection. At the same time, the circulation loop is closed by pose graph optimization. Unlike most visual SLAM systems, LSD-SLAM (J. Engel, T. Sch{umlaut over ( )}ops, and D. Cremers. LSD-SLAM: Large-scale direct monocular SLAM. In 13th European Conference on Computer Vision, Part II, pages 834-849. Springer, 2014) recovers the semi-dense depth map in real time to replace sparse features. Based on the semi-dense depth diagram, LSD-SLAM adopts direct tracking method to locate the camera orientation in real time, which is more robust in the scene without features. SVO (C. Forster, M. Pizzoli, and D. Scaramuzza. SVO: Fast semi-direct monocular visual odometry. In IEEE International Conference on Robotics and Automation, pages 15-22. IEEE, 2014) also adopts the direct tracking method, but with sparse feature points, and thus, high operating efficiency can be achieved.

A major drawback of the traditional key frame-based method is the lacking of robustness for strong rotation and fast motion. First, in order to fully constrain the three-dimensional points, it is required that there should be sufficient parallax between adjacent key frames, but this cannot be satisfied in the case of strong rotation. Secondly, feature matching between key frames with large baselines is time consuming so that the map extension delay is above several frames, and thus, it can only be operated in the background, which also easily leads to tracking loss under fast motion.

In order to improve the tracking robustness for large rotation, ORB-SLAM loosens the parallax requirement when the key frames are inserted. In order to ensure a sparse and accurate global map, redundant key frames and unstable three-dimensional points need to be eliminated later. Both LSD-SLAM and SVO propose to use filtering techniques to obtain robust depth estimation. Filter-based methods are somewhat robust for slow rotations because the parallax in such case is gradually growing larger. For pure rotation motion, similar tracking results can be obtained for any depth value. As the parallax increases slowly, the depth can be updated step by step and eventually converges to a good estimation value. But if the camera motions quickly, the processing difficulty will increase significantly. First of all, these methods are not able to solve the delay problem in map extension very well. If the stereo matching and depth filtering of the background thread cannot keep up with the fast motion of the camera, the tracking will fail. Secondly, strong rotation and fast motion make robust stereo matching very difficult. Since the search range and the distortion of the image block become unpredictable, the feature tracking based on template matching adopted by PTAM and SVO becomes unreliable. The invariant feature used by ORB-SLAM and RDSLAM describes that it is also sensitive to large perspective distortion and motion blur. ASIFT (J.-M. Morel and G. Yu. ASIFT: A new framework for fully affine invariant image comparison. SIAM Journal on Imaging Sciences, 2 (2):438-469, 2009) proposes to handle this problem by simulating different perspectives, but the huge computational cost does not apply to real-time applications.

Of course, there are now some SLAM methods that explicitly handle pure rotation problems. Gauglitz et al. (S. Gauglitz, C. Sweeney, J. Ventura, M. Turk, and T. Hollerer. Live tracking and mapping from both general and rotationonly camera motion. In IEEE International Symposium on Mixed and Augmented Reality, pages 13-22. IEEE, 2012) proposes to switch tracking at six degrees of freedom or panoramic tracking between the current frame and the most recent key frame to obtain a panoramic map and a plurality of scattered three-dimensional sub-maps. Pirchheim et al. (C. Pirchheim, D. Schmalstieg, and G. Reitmayr. Handling pure camera rotation in keyframe-based SLAM. In IEEE International Symposium on Mixed and Augmented Reality, pages 229-238, 2013) and Herrera et al. (C. Herrera, K. Kim, J. Kannala, K. Pulli, J. Heikkila, et al. DT-SLAM: Deferred triangulation for robust SLAM. In IEEE International Conference on 3D Vision, volume 1, pages 609-616, 2014) proposed a similar idea to get a global three-dimensional map. Pirchheim et al. set the depth of each two-dimensional feature point to infinity when performing tracking at six degrees of freedom. Herrera et al. proposed that for two-dimensional features, the re-projection error is replaced by the condition of minimizing the distance as much as possible. Both methods triangulate new three-dimensional points once sufficient parallax is detected. However, pure rotation rarely occurs in practice. It is difficult to avoid camera translation even when the user tries to rotate the camera around the optical center. Panoramic tracking will wrongly consider this translation as an additional rotational component, thereby causing drift. Secondly, the lack of three-dimensional constraints may easily lead to inconsistent scales. Moreover, none of these methods can handle the fast movement of the camera.

Recently, many SLAM systems have improved robustness by combining IMU data, which is referred as visual-inertial fusion SLAM (VI-SLAM). Most VI-SLAM methods are based on filtering. In order to break through the non-extensionality limitation of the filtering methods, MSCKF (A. I. Mourikis and S. I. Roumeliotis. A multi-state constraint kalman filter for vision-aided inertial navigation. In IEEE International Conference on Robotics and Automation, pages 3565-3572. IEEE, 2007) transformed the binary relationship constraint between the three-dimensional point and the camera into a multivariate constraint among multiple cameras. The state vector contains only camera motion and does not contain 3D point positions, thus achieving a linear computational complexity for the number of feature points. Li and Mourikis (M. Li and A. I. Mourikis. High-precision, consistent EKFbased visual-inertial odometry. The International Journal of Robotics Research, 32 (6):690-711, 2013) analyzed the system observability of MSCKF and proposed to fix the linearized points using the Jacobian matrix (G. P. Huang, A. I. Mourikis, and S. I. Roumeliotis. Analysis and improvement of the consistency of extended kalman filter based SLAM. In IEEE International Conference on Robotics and Automation, pages 473-479, 2008) obtained from the first estimation, so as to improve the system consistency of MSCKF. In (J. A. Hesch, D. G. Kottas, S. L. Bowman, and S. I. Roumeliotis. Consistency analysis and improvement of vision-aided inertial navigation. IEEE Transactions on Robotics, 30 (1): 158-176, 2014), the unobservable direction is explicitly specified, thus avoiding false information fusion and reducing system inconsistency. Furthermore, there are also some VI-SLAM methods based on nonlinear optimization, which express the problem as a factor graph and solve it with the Maximum A Posteriori (MAP) estimation. In order to achieve real-time optimization efficiency, only one local map can be maintained, such as (T.-C. Dong-Si and A. I. Mourikis. Motion tracking with fixed-lag smoothing: Algorithm and consistency analysis. In IEEE International Conference on Robotics and Automation, pages 5655-5662. IEEE, 2011) and (S. Leutenegger, S. Lynen, M. Bosse, R. Siegwart, and P. Furgale. Keyframe-based visual-inertial odometry using nonlinear optimization. The International Journal of Robotics Research, 34 (3):314-334, 2015), besides, it is also possible to use incremental optimization methods such as (M. Kaess, H. Johannsson, R. Roberts, V. Ila, J. J. Leonard, and F. Dellaert. iSAM2: Incremental smoothing and mapping using the bayes tree. International Journal of Robotics Research, 31 (2):216-235, 2012), (V. Indelman, S. Williams, M. Kaess, and F. Dellaert. Information fusion in navigation systems via factor graph based incremental smoothing. Robotics and Autonomous Systems, 61 (8):721-738, 2013) and (C. Forster, L. Carlone, F. Dellaert, and D. Scaramuzza. IMU preintegration on manifold for efficient visual-inertial maximum-a-posteriori estimation. In Robotics: Science and Systems, 2015). Most of these methods are aimed at robot navigation and have high quality requirements for IMU.

Under the assumption of smooth motion, Lovegrove et al. proposed to adopt cubic B-spline to parameterize the camera trajectory. With this representation, the camera orientation at any time and the corresponding first and second derivatives can be obtained by interpolation, so that the IMU measurement data can be easily integrated. The rolling shutter phenomenon can also be compensated well with the continuous-time representation. Kerl et al. (C. Kerl, J. Stuckler, and D. Cremers. Dense continuous-time tracking and mapping with rolling shutter RGB-D cameras. In IEEE International Conference on Computer Vision, pages 2264-2272, 2015) also adopted cubic B-spline and direct tracking method to track RGB-D cameras of rolling shutters, and create dense maps. However, strong rotation and fast motion violate the assumption of smooth motion. In addition, motion blur is usually much more obvious than the rolling shutter phenomenon during fast motion. Meilland et al. proposed a unified method to simultaneously estimate motion blur and rolling shutter deformation. But this method is for RGB-D cameras, and a three-dimensional model is required for monocular cameras.

SUMMARY

In view of the deficiencies in the related art, an object of the present disclosure is to provide a method for simultaneous localization and mapping, which reliably handles strong rotation and fast motion, thereby ensuring a good augmented reality experience.

The object of the present disclosure is achieved by the following technical solutions:

A method for simultaneous localization and mapping, includes steps of:

-   -   1) a foreground thread processing a video stream in real time,         and extracting a feature point for any current frame I_(i);     -   2) tracking a set of global homography matrices H_(i) ^(G);     -   3) using a global homography matrix and a specific plane         homography matrix to track a three-dimensional point so as to         obtain a set of 3D-2D correspondences required by a camera         attitude estimation; and     -   4) evaluating quality of the tracking according to a number of         the tracked feature points and classifying the quality into         good, medium and poor;     -   4.1) when the quality of the tracking is good, performing         extension and optimization of a local map, and then determining         whether to select a new key frame;     -   4.2) when the quality of the tracking is medium, estimating a         set of local homography matrices H_(k→i) ^(L), and re-matching a         feature that has failed to be tracked; and     -   4.3) when the quality of the tracking is poor, triggering a         relocating program, and once the relocating is successful,         performing a global homography matrix tracking using a key frame         that is relocated, and then performing tracking of features         again,     -   wherein in step 4.1), when it is determined to select a new key         frame, the selected new key frame wakes up a background thread         for global optimization, the new key frame and a new         triangulated three-dimensional point is added for extending a         global map, and a local bundle adjustment is adopted for         optimization; then an existing three-dimensional plane is         extended, the added new three-dimensional point is given to the         existing plane, or a new three-dimensional plane is extracted         from the added new three-dimensional point; subsequently, a loop         is detected by matching the new key frame with an existing key         frame; and finally, a global bundle adjustment is performed on         the global map.

In an embodiment, the step 2) includes:

-   -   for two adjacent frames of images, estimating a global         homography transformation from a previous frame of image I_(i−1)         to a current frame of image I_(i) by directly aligning the         previous frame of image with the current frame of image as:

${H_{{({i - 1})}\rightarrow i}^{G} = {\underset{H}{\arg \; \min}{\sum\limits_{X \in \Omega}{{{{\overset{\sim}{I}}_{i - 1}(x)} - {{\overset{\sim}{I}}_{i}\left( {\pi \left( {\overset{\sim}{H}x^{h}} \right)} \right)}}}_{\delta_{1}}}}},$

where Ĩ_(i−1) and Ĩ_(i) are small blurred images of I_(i−1) and I_(i) respectively, Ω is a small image domain, a superscript h indicates that a vector is a homogeneous coordinate, a tilde in {tilde over (H)} indicates that a homography matrix H is converted from a source image space to a small image space, and ∥⋅∥_(δ)represents a Huber function; and a calculation method is expressed as the following formula:

$\begin{matrix} {{e}_{\delta} = \left\{ {{\begin{matrix} \frac{{e}_{2}^{2}}{2\; \delta} & {{{if}\mspace{14mu} {e}_{2}} \leq \delta} \\ {{e}_{2} - \frac{\delta}{2}} & {otherwise} \end{matrix}.};} \right.} & (4) \end{matrix}$

for each key frame F_(k), which has an initial solution to the global homography matrix H_(k→(i−1)) ^(G) of I_(i−1), using a set of feature matchings (denoted as M_(k,i−1)={(X_(k), X_(i−1))}) between frames F_(k) and I_(i−1)to determine a set of key frames which has an overlap with I_(i−1) and is denoted as K_(i−1), selecting five key frames satisfying that |M_(k,i−1)|>20 and |M_(k,i−1)| is maximized, optimizing an initial solution H_(k→(i−1)) ^(G) for each key frame F_(k) in K_(i−1):

${H_{k\rightarrow{({i - 1})}}^{G} = {\underset{H}{\arg \; \min}{\sum\limits_{X \in \Omega}\left( {{{{{\overset{\sim}{F}}_{k}(x)} - {{\overset{\sim}{I}}_{i - 1}\left( {\pi \left( {\overset{\sim}{H}x^{h}} \right)} \right)}}}_{\delta_{1}} + {\sum\limits_{{({x_{k},x_{i - 1}})} \in M_{k,{j - 1}}}{{{\pi \left( {\overset{\sim}{H}x_{k}^{h}} \right)} - x_{i - 1}}}_{\delta_{k}}}} \right)}}},$

and

obtaining the global homography transformation H_(k→t) ^(G)=H_((i−1)→i) ^(G)H_(k→(i−1)) ^(G) from the key frame F_(k) to the current frame I_(i) through transitivity.

In an embodiment, the step 3) includes:

for a feature point which is fully constrained and has a reliable three-dimensional position, determining a search position by predicting a camera orientation of a current frame C_(i), predicting C_(i) using the global homography matrix H_(i) ^(G), denoting a two-dimensional feature point of a frame F_(k)∈K_(i−1) as x_(k) and denoting a corresponding three-dimensional point as X_(k), obtaining a three-dimensional to two-dimensional correspondence (X_(k),x_(i)) through a formula x_(i)=π(H_(k→i) ^(G)x_(k) ^(h)), in which a set of all the three-dimensional to two-dimensional correspondences is denoted as V_(c) _(i) ={(X_(j),x_(j))}, and predicting a camera position by solving

$\begin{matrix} {{\underset{C_{i}}{\arg \; \min}{\sum\limits_{{({X_{j},x_{j}})} \in V_{C_{i}}}{{{\pi \left( {K\left( {R_{i}\left( {X_{j} - p_{i}} \right)} \right)} \right)} - x_{j}}}_{\delta_{x}}}};} & (6) \end{matrix}$

and for a feature point which is not fully constrained and has no reliable three-dimensional position, first of all, directly using π(H_(k→i) ^(G)x_(k) ^(h)) as the search position, and obtaining a set of local homography matrices H_(k→i) ^(L) according to estimation of matched correspondence if the quality of the tracking is evaluated as medium, while for an unmatched feature, trying one by one in the search position {H_(k→i) ^(L) ^(j) x_(k)|H_(k→i) ^(L) ^(j) ∈H_(k→i) ^(L)}, in which for each search position, its range is a r×r square area centered on the search position and r is 10 and 30 respectively for points fully constrained and points not fully constrained; and

-   -   given a homography matrix H_(k→i), calculating a change of four         vertices of each aligned image block, aligning a local block         near x_(k) in the key frame F_(k) to the current frame I_(i)         through a formula κ(y)=I_(k)(H_(k→i) ⁻¹(π(H_(k→i)x_(k)         ^(h)+y^(h)))) (7) only when the change exceeds a certain         threshold, wherein κ(y) is an image gray scale for an offset of         y relative to a center of the block, and y∈(−W/2, W/2)×(−W/2,         W/2), where W is a dimension of the image block; and calculating         a value of a zero-mean SSD for a FAST corner point extracted         within a search area, and selecting the best one when the offset         is smaller than a certain threshold.

In an embodiment, said performing extension and optimization of the local map in step 4.1) includes:

-   -   for one two-dimensional feature x_(k) of a frame F_(k),         calculating a ray angle according to a formula of

$\begin{matrix} {{\alpha \left( {i,k} \right)} = {{acos}\left( \frac{r_{k}^{T}r_{i}}{{r_{k}} \cdot {r_{i}}} \right)}} & (8) \end{matrix}$

once a corresponding feature x_(i) is found in a frame I_(i), where r_(j)=R_(j) ^(T)K⁻¹x_(j) ^(h); if α(i,k)≥δ_(α), obtaining a three-dimensional position X using a triangulation algorithm, and otherwise, specifying an average depth d_(k) of the key frame F_(k) for x_(k) to initialize X, X=d_(k)K⁻¹x_(k) ^(h), and optimizing X according to a formula of

$\begin{matrix} {{{\underset{X}{argmin}{{{\pi \left( {{KR}_{k}\left( {X - p_{k}} \right)} \right)} - x_{k}}}_{2}^{2}} + {{{\pi \left( {{KR}_{i}\left( {X - p_{i}} \right)} \right)} - x_{i}}}_{2}^{2}};} & (9) \end{matrix}$

selecting a local window, fixing a camera orientation first, individually optimizing each three-dimensional point position X_(i) by solving

$\begin{matrix} {{\underset{X_{i}}{\arg \; \min}{\sum\limits_{j \in V_{X_{i}}}{{{\pi \left( {K\left( {R_{j}\left( {X_{i} - p_{j}} \right)} \right)} \right)} - x_{ij}}}_{\delta_{x}}}},} & (10) \end{matrix}$

where V_(x) _(i) , is a set of frame number indices of the key frame and a frame where the three-dimensional point X_(i) is observed in a sliding window, and then fixing the three-dimensional point position and optimizing all camera orientations in the local window.

In an embodiment, said fixing the three-dimensional point position and optimizing all camera orientations in the local window includes:

-   -   assuming that there is already a linear acceleration â and a         rotation speed {circumflex over (ω)} measured in a local         coordinate system, and a real linear acceleration is         a=â−b_(a)+n_(a), a real rotation speed is ω={circumflex over         (ω)}−b_(ω)+n_(ω), n_(a)˜N(0,σ_(n) _(a) ²I), n_(ω)˜N(0,σ_(n) _(ω)         ²I) are Gaussian noise of inertial measurement data, I is a 3×3         identity matrix, b_(a) and b_(ω)are respectively offsets of the         linear acceleration and the rotation speed with time, extending         a state of a camera motion to be: s=[q^(T) p^(T) v^(T) b_(a)         ^(T) b_(ω) ^(T)]^(T), where v is a linear velocity in a global         coordinate system, a continuous-time motion equation of all         states is:

$\begin{matrix} {{\overset{.}{q} = {\frac{1}{2}{\Omega (\omega)}q}},{\overset{.}{p} = v},{\overset{.}{v} = {R^{T}a}},{{\overset{.}{b}}_{a} = w_{a}},{{\overset{.}{b}}_{\omega} = w_{\omega}},} & (15) \end{matrix}$

where

${{\Omega (\omega)} = \begin{bmatrix} {- \lbrack\omega\rbrack_{x}} & \omega \\ {- \omega^{T}} & 0 \end{bmatrix}},$

[⋅]_(x) represents a skew symmetric matrix, w_(a)˜N(0, σ_(w) ²I) and w_(ω)˜N(0, σ_(w) _(a) ²I) represent random walk processes, and a discrete-time motion equation of a rotational component is:

q _(i+1) =q _(Δ)(ω_(i) ,t _(Δ) _(i) )⊗q _(i)   (17),

where

$\begin{matrix} {{q_{\Delta}\left( {\omega,t_{\Delta}} \right)} = \left\{ {\begin{matrix} \begin{bmatrix} {\frac{\omega}{\omega }{\sin \left( {\frac{\omega }{2}t_{\Delta}} \right)}} \\ {\cos \left( {\frac{\omega }{2}t_{\Delta}} \right)} \end{bmatrix} & {{{if}\mspace{14mu} {\omega }} > ɛ_{\omega}} \\ \begin{bmatrix} {\frac{\omega }{2}t_{\Delta}} \\ 1 \end{bmatrix} & {{otherwis}e} \end{matrix},} \right.} & (18) \end{matrix}$

t_(Δ) _(i) is a time interval between two adjacent key frames i and i+1, ⊗ is a quaternion multiplication operator, ε_(ω) is a small value that prevents zero division; and approximating a real value of q_(Δ)(ω_(i),t_(Δ) _(i) ) by a formula of

$\begin{matrix} {{{q_{\Delta}\left( {\omega_{i},t_{\Delta_{i}}} \right)} \approx {\begin{bmatrix} \frac{\overset{\sim}{\theta}}{2} \\ 1 \end{bmatrix} \otimes {q_{\Delta}\left( {{{\hat{\omega}}_{i} - b_{\omega_{i}}},t_{\Delta_{i}}} \right)}}},} & (19) \end{matrix}$

where {circumflex over (θ)} is a 3×1 error vector, yielding: {circumflex over (θ)}≈2[q₆₆({circumflex over (ω)}_(i)−b_(ω) _(i) +n_(ω),t₆₆ _(i) )⊗q_(Δ) ⁻¹({circumflex over (ω)}_(i)−b_(ω) _(i) ,t_(Δ) _(i) )]_(1.3)≈2G_(ω),n_(ω), (20), where Gω is a Jacobian matrix associated with noise n_(ω), further yielding {circumflex over (θ)}≈2[q_(i+1)⊗q_(i) ⁻¹⊗q_(Δ) ⁻¹({circumflex over (ω)}_(i)−b_(ω) _(i) , t_(Δ) _(i) )]_(1.3) (21), and defining a cost function and a covariance of the rotational component as:

e _(q)(q _(i) ,q _(i+1), b_(ω) _(i) )=[q _(i+1) ⊗q _(i) ⁻¹ ⊗q _(Δ) ⁻¹({circumflex over (ω)}_(i)−b_(ω) _(i) ,t _(Δ) _(i) )]_(1.3),Σ_(q)=σ_(n) _(a) ² G _(ω) G _(ω) ^(T)   (22);

defining discrete-time motion equations, cost functions and covariance for other components as:

$\begin{matrix} {\mspace{79mu} {{p_{i + 1} = {p_{i} + {v_{i}t_{\Delta_{i}}} + {R_{i}^{T}{p_{\Delta}\left( {a_{i},t_{\Delta_{i}}} \right)}}}}\mspace{79mu} {v_{i + 1} = {v_{i} + {R_{i}^{T}{v_{\Delta}\left( {a_{i},t_{\Delta_{i}}} \right)}}}}\mspace{79mu} {b_{a_{i + 1}} = {b_{a_{i}} + {w_{a}t_{\Delta_{i}}}}}\mspace{79mu} {b_{\omega_{i + 1}} = {b_{\omega_{i}} + {w_{\omega}t_{\Delta_{i}}}}}{{e_{p}\left( {q_{i},p_{i},p_{i + 1},v_{i},b_{a_{i}}} \right)} = {{R_{i}\left( {p_{i + 1} - p_{i} - {v_{i}t_{\Delta_{i}}}} \right)} - {p_{\Delta}\left( {{\hat{a}}_{i} - {b_{a_{i},}t_{\Delta_{i}}}} \right)}}}\mspace{79mu} {{e_{v}\left( {q_{i},v_{i},v_{i + 1},v_{i},b_{a_{i}}} \right)} = {{R_{i}\left( {v_{i + 1} - v_{i}} \right)} - {v_{\Delta}\left( {{{\hat{a}}_{i} - b_{a_{i}}},t_{\Delta_{i}}} \right)}}}\mspace{79mu} {{e_{b_{a}}\left( {b_{a_{i}},b_{a_{i + 1}}} \right)} = {b_{a_{i + 1}} - b_{a_{i}}}}\mspace{79mu} {{e_{b_{\omega}}\left( {b_{\omega_{i}},b_{\omega_{i + 1}}} \right)} = {b_{\omega_{i + 1}} - b_{\omega_{i}}}}\mspace{79mu} {\sum_{p}{= {\frac{1}{4}\sigma_{n_{a}}^{2}t_{\Delta_{i}}^{4}I}}}\mspace{79mu} {\sum_{v}{= {\sigma_{n_{a}}^{2}t_{\Delta_{i}}^{2}I}}}\mspace{79mu} {\sum_{b_{a}}{= {\sigma_{w_{a}}^{2}t_{\Delta_{i}}^{2}I}}}\mspace{79mu} {{\sum_{b_{\omega}}{= {\sigma_{w_{\omega}}^{2}t_{\Delta_{i}}^{2}I}}},}}} & (25) \end{matrix}$

where

$\begin{matrix} {{{p_{\Delta}\left( {a,t_{\Delta}} \right)} = {\frac{1}{2}{at}_{\Delta}^{2}}},{{{v_{\Delta}\left( {a,t_{\Delta}} \right)} = {at}_{\Delta}};}} & (24) \end{matrix}$

defining an energy function of all motion states s₁ . . . s_(l) in the sliding window as:

$\begin{matrix} {{{\underset{s_{i}\ldots \mspace{14mu} s_{l}}{\arg \; \min}{\sum\limits_{i = 1}^{l}\; {\sum\limits_{j \in V_{C_{i}}}^{\;}\; {{{\pi \left( {K\left( {R_{i}\left( {X_{j} - p_{i}} \right)} \right)} \right)} - x_{ij}}}}}} + {\sum\limits_{i = 1}^{l - 1}\; {{e_{q}\left( {q_{i},q_{i + 1},b_{\omega_{i}}} \right)}}_{\sum_{q}}^{2}} + {\sum\limits_{i = 1}^{l - 1}\; {{e_{p}\left( {q_{i},p_{i},p_{i + 1},v_{i},b_{a_{i}}} \right)}}_{\sum_{p}}^{2}} + {\sum\limits_{i = 1}^{l - 1}\; {{e_{v}\left( {q_{i},v_{i},v_{i + 1},v_{i},b_{a_{i}}} \right)}}_{\sum_{v}}^{2}} + {\sum\limits_{i = 1}^{l - 1}\; {{e_{b_{a}}\left( {b_{a_{i}},b_{a_{i + 1}}} \right)}}_{\sum_{b_{a}}}^{2}} + {\sum\limits_{i = 1}^{l - 1}\; {{e_{b_{\omega}}\left( {b_{\omega_{i}},b_{\omega_{i + 1}}} \right)}}_{\sum_{b_{\omega}}}^{2}}},} & (26) \end{matrix}$

where l is a size of the sliding window and ∥e∥_(Σ) ²=e^(T)Σ⁻¹e is a square of a Mahalanobis distance;

for a case without an IMU sensor, setting â_(i)=0, obtaining one optimal angular velocity by using the feature matching to align successive frame images:

$\begin{matrix} {{{\hat{\omega}}_{i} = {\underset{\omega}{argmin}\left( {{\sum\limits_{x \in \Omega}{{{{\overset{\sim}{I}}_{i}(x)} - {{\overset{\sim}{I}}_{i + 1}\left( {\pi \left( {{{KR}_{\Delta}\left( {\omega,t_{\Delta_{i}}} \right)}K^{- 1}x^{h}} \right)} \right)}}}_{\delta_{I}}} + {\sum\limits_{{({x_{i},x_{i + 1}})} \in M_{i,{i + 1}}}{\frac{1}{\delta_{X}}{{{\pi \left( {{{KR}_{\Delta}\left( {\omega,t_{\Delta_{i}}} \right)}K^{- 1}x_{i}^{h}} \right)} - x_{i + 1}}}_{2}^{2}}}} \right)}},} & (27) \end{matrix}$

where R_(Δ)(ω,t_(Δ)) is a rotation matrix of the formula (18), M_(i,i+1) is a set of feature matchings between images i and i+1, denoting a set of IMU measurement data between frames i and i+1 as {(â_(ij), {circumflex over (ω)}_(ij), t_(ij))|j=1 . . . n_(i)} if there is real IMU measurement data, where â_(ij), {circumflex over (ω)}_(ij) and t_(ij) are respectively a j_(th) linear acceleration, a rotation speed and a timestamp; and using a pre-integration technique to pre-integrate the IMU measurement data, and using the following formulas to replace components in (18) and (24):

$\mspace{20mu} {{q_{\Delta}\left( \left\{ \left( {{{\hat{\omega}}_{ij} - b_{\omega_{i}}},t_{ij}} \right) \right\} \right)} \approx {{q_{\Delta}\left( {{{\hat{\omega}}_{ij} - {\hat{b}}_{\omega_{i}}},t_{ij}} \right)} + {\frac{\partial q_{\Delta}}{\partial b_{\omega}}\left( {b_{\omega} - {\hat{b}}_{\omega}} \right)}}}$ ${{p_{\Delta}\left( \left\{ \left( {{{\hat{a}}_{ij} - b_{a_{i}}},{{\hat{\omega}}_{ij} - b_{\omega_{i}}},t_{ij}} \right) \right\} \right)} \approx {{p_{\Delta}\left( \left\{ \left( {{{\hat{a}}_{ij} - {\hat{b}}_{a_{i}}},{{\hat{\omega}}_{ij} - {\hat{b}}_{\omega_{i}}},t_{ij}} \right) \right\} \right)} + {\frac{\partial p_{\Delta}}{\partial b_{a}}\left( {b_{a} - {\hat{b}}_{a}} \right)} + {\frac{\partial p_{\Delta}}{\partial b_{\omega}}\left( {b_{\omega} - {\hat{b}}_{\omega}} \right)}}},{{v_{\Delta}\left( \left\{ \left( {{{\hat{a}}_{ij} - b_{a_{i}}},{{\hat{\omega}}_{ij} - b_{\omega_{i}}},t_{ij}} \right) \right\} \right)} \approx {{v_{\Delta}\left( \left\{ \left( {{{\hat{a}}_{ij} - {\hat{b}}_{a_{i}}},{{\hat{\omega}}_{ij} - {\hat{b}}_{\omega_{i}}},t_{ij}} \right) \right\} \right)} + {\frac{\partial v_{\Delta}}{\partial b_{a}}\left( {b_{a} - {\hat{b}}_{a}} \right)} + {\frac{\partial v_{\Delta}}{\partial b_{\omega}}\left( {b_{\omega} - {\hat{b}}_{\omega}} \right)}}}$

where {circumflex over (b)}_(a) _(i) ; and {circumflex over (b)}_(ω) _(i) are states of b_(ω) _(i) and b_(ω) _(i) , when being pre-integrated.

In an embodiment, the global optimization by the background thread includes:

for each three-dimensional point in the new key frame F_(k), calculating a maximum ray angle with respect to the existing key frame using the formula (8) first; marking the point as a fully constrained three-dimensional point if max_(i) α(i,k)≥δ_(α) and three-dimensional coordinates are successfully triangulated; then extending the existing three-dimensional plane visible in the frame F_(k) by the new and fully constrained three-dimensional point X belonging to the plane; defining a neighborhood relationship by performing Delaunay triangulation on the two-dimensional feature in the key frame F_(k); when a three-dimensional point satisfies the following three conditions at the same time, adding this point to a set V_(P) of the plane P: 1) X is not assigned to any plane, 2) X is connected with another point in V_(P), and 3)|n^(T)X+d|<(δ_(P), where d_(k) is the average depth of the key frame F_(k); adopting a RANSAC algorithm to extract a new three-dimensional plane; randomly sampling three connected points in each iteration to initialize a set V_(P) of three-dimensional interior points; adopting SVD to recover a three-dimensional plane parameter P, and then starting an inner loop which iteratively adjusts extension and optimization of the plane; for each internal iteration, checking a distance from the three-dimensional points X, which are connected to the points in the set V_(P), to the plane; if |n^(T)X+d|<δ_(P), adding X to V_(P), optimizing the plane by a minimization formula for all points in V_(P):

$\begin{matrix} {{\underset{P}{argmin}{\sum\limits_{X \in V_{P}}\left( {{n^{T}X} + d} \right)^{2}}},} & (28) \end{matrix}$

and iteratively extending V_(P) by repeating the above steps; and when |V_(P)|<30, discarding P and V_(P); sorting the extracted planes in a descending order according to a number of relevant three-dimensional points, and denoting a set of the sorted interior points as {V_(P) _(i) =1 . . . N_(P)}; starting from a first one, for any V_(P) _(i) , if a three-dimensional point X∈V_(P) _(i) also exists in a previous set of plane points V_(P) _(j) , (j<i), removing this point from V_(P) _(i) ; and optimizing the three-dimensional plane parameter by adding a point-to-plane constraint in the global bundle adjustment,

$\begin{matrix} {{\underset{C,X,P}{argmin}{\sum\limits_{i = 1}^{N_{F}}{\sum\limits_{j \in V_{i}}{{{\pi \left( {K\left( {R_{i}\left( {X_{j} - p_{i}} \right)} \right)} \right)} - x_{ij}}}_{\delta_{x}}}}} + {\sum\limits_{i = 1}^{N_{P}}{\sum\limits_{j \in V_{P_{i}}}{{{{n_{i}^{T}X_{j}} + d_{j}}}_{\delta_{P}}.}}}} & (29) \end{matrix}$

In an embodiment, the method for simultaneous localization and mapping further includes an initialization step of placing a virtual object on a specific plane of a real scene in a real size:

initializing the tracking using a planar marker of a known size, providing a world coordinate system at a center of the planar marker, adopting n three-dimensional points X_(i), i=1, . . . , n uniformly along an edge of the planar marker, and recovering an orientation (R,t) of a camera in an initial frame by solving the following optimization equation:

$\begin{matrix} {{{\underset{R,t}{argmin}{\sum\limits_{i = 1}^{n}{{{I\left( {\pi \left( {{RX}_{i} + t} \right)} \right)} - {I\left( {\pi \left( {{R\left( {X + {\delta \; X_{i}^{}}} \right)} + t} \right)} \right)}}}^{2}}} + {{{I\left( {\pi \left( {{RX}_{i} + t} \right)} \right)} - {I\left( {\pi \left( {{R\left( {X + {\delta \; X_{i}^{\bot}}} \right)} + t} \right)} \right)} + 255}}^{2}},} & (30) \end{matrix}$

where directions of δX_(∥)and δX_(⊥) are respectively a direction along a tangent at X_(i) of the edge and a vertically inward direction at X_(i), and lengths thereof ensure that distances between projection points of X+δX_(i) ^(∥) and X+δX_(i) ^(⊥) on the image and a projection point of X_(i) are one pixel; constructing a 4-layer pyramid for the image by adopting a coarse-to-fine strategy to enlarge a convergence range, starting from a top layer, and using an optimized result as an initial value of a lower layer optimization.

The beneficial effects of the present disclosure are:

1. The method of the present disclosure provides a new simultaneous localization and mapping framework, which advances the extension and optimization of partial local map to the foreground thread, thereby improving the robustness against strong rotation and fast motion.

2. The method of the present disclosure provides a method for feature tracking based on multiple homography matrices, which can be used for tracking a three-dimensional feature point that is fully constrained and sufficient to recover the three-dimensional position accurately, and can also be used for feature-tracking a point that is not fully constrained and has unreliable three-dimensional position. This method not only can handle strong rotation and fast motion, but also can achieve real-time computational efficiency, and can further run in real time on mobile devices.

3. The method of the present disclosure provides a new local map optimization method based on a sliding window, which can effectively optimize the camera orientation by reasonably simulating the IMU measurement value, and significantly improve the robustness in the case of rapid camera motion and severe blurring. This optimized framework can also be extended to the case where actual IMU data is incorporated, so as to further improve robustness and accuracy.

4. The method of the present disclosure provides a method of obtaining a real scale of a particular plane and scene, thereby placing a virtual object on a particular plane in a real size. This method only requires the user to align the size of a known marker to initiate tracking without the need to capture the marker texture in advance, so it is more convenient to use.

BRIEF DESCRIPTION OF DRAWINGS

FIG. 1 is a system framework diagram of the method RKSLAM herein. It's important to note that unlike most key frame-based systems that put all the mapping work in the background, the present method puts the local map extension and optimization in the foreground in order to handle strong rotation and fast motion.

FIG. 2 is a comparison diagram of the global homography matrix estimation. (a) Current image. (b) Selected key frame that needs matching. (c) Align (b) to (a) by solving formula (3). (d) Align (b) to (a) by solving formula (5). (e) A combination of (a) and (c). (f) A combination of (a) and (d).

FIG. 3 is an effect diagram of a partial homography matrix for feature matching. Upper: 37 pairs of matching obtained by a global homography matrix and a specific plane homography matrix. Lower: 153 pairs of matching obtained by local homography matrix estimation.

FIG. 4 is a three-dimensional plane extracted and obtained by fitting the found three-dimensional points.

FIG. 5 is a diagram that compares the effect of the SLAM method in an indoor scene in the way of inserting a virtual cube. (a) RKSLAM result without a priori motion constraint. (b) RKSLAM result when setting â_(i)=0 and {circumflex over (ω)}_(i)=0. (c) RKSLAM result with estimated rotation speed. (d) RKSLAM result when actual IMU data is incorporated. (e)Result of ORB-SLAM. (f) Result of PTAM. (g) Result of LSD-SLAM.

DESCRIPTION OF EMBODIMENTS

The present disclosure discloses a method for simultaneous localization and mapping, which can reliably handle strong rotation and fast motion, thereby ensuring a good experience of augmented reality. In this method, a new feature tracking method based on multiple homegraphy matrix is proposed, which is efficient and robust under strong rotation and fast motion. Based on this, a real-time local map extension strategy is further proposed, which can triangulate three-dimensional points of a scene in real time. Moreover, a camera orientation optimization framework based on a sliding window is also proposed to increase priori motion constraints between successive frames using simulated or actual IMU data.

Before describing the present method in detail, the convention of the mathematical formula in the method is introduced (same as R. Hartley and A. Zisserman. Multiple view geometry in computer vision. Cambridge university press, 2004). A scalar is represented by an italic character (such as x), and a matrix is represented by a bold uppercase letter (such as H); a column vector is represented by a bold letter (such as b), and a transpose b^(⊥) thereof represents a corresponding row vector. In addition, a set of points, vectors, and matrices is generally represented by an italic uppercase letter (such as V). For the vector x, the superscript h represents the corresponding homogeneous coordinate, such as X^(h). In the method, each three-dimensional point i contains a three-dimensional position X_(i) and a local image block X_(i). Each frame i holds its image I_(i), camera orientation C_(i), a set of two-dimensional feature point positions {x_(ij)}, and a set of corresponding three-dimensional point indices V_(i). A set of selected key frames is represented by {F_(k)|k=1 . . . N_(F)}. The camera orientation is parameterized as a quaternion q_(i) and a camera position p_(i). For a three-dimensional point X_(j), its two-dimensional projection in the i_(th) image is calculated by the following formula: x_(ij)=π(K(R_(i)(X_(j)−p_(i)))) (1), where K is the camera internal parameter which is known and constant by default, and R_(i) is a rotation matrix of q_(i), while π(⋅)is a projection function π([x,y,z]^(T))=[x/z,y/z]^(T). Each plane i holds its parameter P_(i) and an index of a three-dimensional point belonging to it, which is marked as V_(p) _(i) . The plane parameter is defined as P_(i)=[n_(i) ^(T),d_(i)]^(T), where n_(i) represents a normal direction thereof and d_(i) represents a symbol distance from an origin to the plane. For the three-dimensional point X on the plane, the formula n_(i) ^(T)X+d_(i)=0 is satisfied. The system maintains three types of homography matrices, which are used to describe a transformation from the key frame to the current frame. These three types of homography matrices are respectively: a global homography matrix, a local homography matrix and a specific plane homography matrix. The global homography matrix is used for its entire image. The global homography matrix from the key frame F_(k) to the current frame I_(i) is expressed as H_(k→i) ^(G), and a set of global homography matrices of all key frames is expressed as H_(i) ^(G)={H_(k→i) ^(G)|k=1 . . . N_(F)}. The local homography matrix is used to align two local image blocks. For two frames (F_(k),I_(i)), multiple local homography matrices can be obtained and expressed as H_(k→i) ^(L)={H_(k→i) ^(L) ^(j) |j=1 . . . N_(k→i) ^(L)}. The specific plane homography matrix is also used to align an image area of a specific three-dimensional plane. For the three-dimensional plane P_(j) existing in the key frame F_(k), its corresponding homography matrix from F_(k) to I_(i) can be written as

$\begin{matrix} {H_{k\rightarrow i}^{P_{j}} = {{K\left( {{R_{i}R_{k}^{T}} + \frac{{R_{i}\left( {p_{i} - p_{k}} \right)}n_{j}^{T}R_{k}^{T}}{d_{j} + {n_{j}^{T}R_{k}p_{k}}}} \right)}{K^{- 1}.}}} & (2) \end{matrix}$

For each key frame F_(k), a set of specific plane homography matrices is solved and expressed as H_(k→i) ^(P)={H_(k→i) ^(P) ^(j) |j=1 . . . N_(k→i) ^(P)}.

For a set of continuously dynamically changing video sequences (FIG. 5), the camera parameters are estimated using the method proposed by the present disclosure, and the camera has strong rotation and fast motion when shooting.

As shown in FIG. 1, the implementation steps are as follows:

-   -   1. The foreground thread processes a video stream in real time.         For any current frame I_(i)a FAST corner point is extracted         first (E. Rosten and T. Drummond. Machine learning for high         speed corner detection. In 9th European Conference on Computer         Vision, Part I, pages 430-443. Springer, 2006), and a set of         global homography matrices H_(i) ^(G) of I_(i) is tracked and         obtained.

The global homography matrix can roughly align the entire image, and the global homography matrix is estimated by directly aligning the key frame F_(k) and the current frame I_(i):

$\begin{matrix} {H_{k\rightarrow i}^{G} = {\underset{H}{argmin}{\sum\limits_{X \in \Omega}{{{{{\overset{\sim}{F}}_{k}(x)} - {{\overset{\sim}{I}}_{i}\left( {\pi \left( {\overset{\sim}{H}x^{h}} \right)} \right)}}}_{\delta_{I}}.}}}} & (3) \end{matrix}$

{tilde over (F)}_(k) and Ĩ_(i) are respectively small blurred images (SBI) of F_(k) and I_(i) (G. Klein and D. W. Murray. Improving the agility of keyframe-based SLAM. In 10th European Conference on Computer Vision, Part II, pages 802-815. Springer, 2008). Ω is a SBI domain, and a superscript h indicates that the vector is a homogeneous coordinate. A tilde in {tilde over (H)} indicates that the homography matrix H is converted from a source image space to a SBI space. δ_(I), ∥⋅∥_(δ)represents a Huber function, and a calculation method is as follows:

$\begin{matrix} {{e}_{\delta} = \left\{ {\begin{matrix} \frac{{e}_{2}^{2}}{2\delta} & {{{{if}\mspace{14mu} {e}_{2}} \leq \delta}\mspace{14mu}} \\ {{e}_{2} - \frac{\delta}{2}} & {{otherwise}.} \end{matrix}.} \right.} & (4) \end{matrix}$

Assuming that a baseline between two adjacent frames is small, a global homography matrix can roughly represent a motion between two frames of images. By solving the formula (3) for the previous frame I_(i−1) and the current frame I_(i), H_((i−1)→i) ^(G) can be obtained. H_(k→i) ^(G)=H_((i−1)→i) ^(G)H_(k→(i−1)) ^(G) ) can be obtained by transitivity. The set of feature matchings (denoted as M_(k,i−1)={(x_(k), x_(i−1))}) between F_(k) and I_(i−1) is used, H_(→(i−1)) ^(G). First, a set of key frames which has an overlapping area with I_(i−1) is determined using M_(k,i−1) and denoted as K_(i−1). Five key frames are selected under conditions that |M_(k,i−1)|>20 and M_(k,i−1)| is maximized. For each key frame F_(k) in K_(i−1), M_(k,i−1) is used to prevent cumulative errors and offsets, and the formula (3) is modified to:

$\begin{matrix} {{H_{k\rightarrow{({i - 1})}}^{G} = {\underset{H}{argmin}{\sum\limits_{X \in \Omega}\left( {{{{{\overset{\sim}{F}}_{k}(x)} - {{\overset{\sim}{I}}_{i - 1}\left( {\pi \left( {\overset{\sim}{H}x^{h}} \right)} \right)}}}_{\delta_{I}} + {\sum\limits_{{({x_{k},x_{i - 1}})} \in M_{k,{i - 1}}}{{{\pi \left( {\overset{\sim}{Hx}}_{k}^{h} \right)} - x_{i - 1}}}_{\delta_{x}}}} \right)}}},} & (5) \end{matrix}$

δ_(x). In the experiment, δ_(I)=0.1, δ_(x)=10. With respect to formula (3), formula (5) can interpolate multiple plane motions in case of a larger baseline. The Gauss-Newton method is used to solve formulas (3) and (5). FIG. 2 is a comparison diagram in which (a) is the current image and (b) is the selected key frame. As shown in the figure, if formula (3) is directly solved, it can be seen from (c) and (e) that an estimation result of the global homography matrix is not good, and the alignment error is very obvious; and on the contrary, the estimation effect of the global homography matrix obtained by the formula (5) is much better, as shown in (d) and (f).

2. The global homography matrix and the specific plane homography matrix are used to track the three-dimensional point, and obtain a set of 3D-2D correspondences required by the camera orientation estimation.

For a point which is fully constrained and has a reliable three-dimensional position, it is only necessary to predict the camera orientation of the current frame C_(i) to project a three-dimensional point and determine the search position. C_(i) is predicted using a global homography matrix H_(i) ^(G). The global homography matrix can be derived from the previous frame I_(i−1), and the set of key frames overlapping with it is In order not to lose generality, any two-dimensional feature of the frame F_(k)∈K_(i−1) is denoted as x_(k) and the corresponding three-dimensional point is denoted as X_(k). Through the formula x_(i)=π(H_(k→i) ^(G),x_(k) ^(h)), a three-dimensional to two-dimensional correspondence (X_(k),xi) can be obtained. A set of all three-dimensional to two-dimensional correspondences is denoted as V_(c) _(i) ={(X_(j), x_(j))}. The camera position can be predicted by a formula of

$\begin{matrix} {\underset{C_{i}}{argmin}{\sum\limits_{{({X_{j},x_{j}})} \in V_{C_{i}}}{{{\pi \left( {K\left( {R_{i}\left( {X_{j} - p_{i}} \right)} \right)} \right)} - x_{j}}}_{\delta_{x}}}} & (6) \end{matrix}$

R_(i). If there is real IMU data, the camera position can also be predicted directly by propagation of the IMU state.

For a point belonging to the three-dimensional plane P_(j), the plane homography matrix H_(k→i) ^(P) ^(j) obtained by formula (2) is used for image block alignment, and used to estimate the camera orientation.

3. The method in the paper (G. Klein and D. W. Murray. Parallel tracking and mapping for small AR workspaces. In 6th IEEE/ACM International Symposium on Mixed and Augmented Reality, pages 225-234, 2007) is used to evaluate quality of the tracking which can be divided into good, medium (not good) and poor (the tracking result may be not good or even poor for fast motion with large translation).

3.1. When the quality of the tracking is good, the tracking result is used directly to extend and optimize the local map, and then corresponding algorithms in papers (G. Klein and D. W. Murray. Parallel tracking and mapping for small AR workspaces. In 6th IEEE/ACM International Symposium on Mixed and Augmented Reality, pages 225-234, 2007) and (G. Klein and D. W. Murray. Improving the agility of key frame-based SLAM. In 10th European Conference on Computer Vision, Part II, pages 802-815. Springer, 2008) are used to determine whether a new key frame is to be selected or not.

For a two-dimensional feature x_(k) of the frame F_(k), once a corresponding feature x_(i) is found in frame I_(i), the three-dimensional point X will be triangulated. Firstly, the ray angle is calculated according to the formula

$\begin{matrix} {{{\alpha \left( {i,k} \right)} = {{acos}\left( \frac{r_{k}^{T}r_{i}}{{r_{k}} \cdot {r_{i}}} \right)}},} & (8) \end{matrix}$

where r_(j)=R_(j) ^(T)K⁻¹x_(j) ^(h). In the ideal case where there is sufficient parallax and α(i, k)≥δ_(α), the conventional triangulation algorithm in (R. Hartley and A. Zisserman. Multiple view geometry in computer vision. Cambridge university press, 2004) is used to obtain X. In the experiment, it is set that δ_(α)=1°. However, in most cases, the parallax in the first observation is often insufficient. But on the other hand, the depth of any x_(k) will result in similar re-projection results in frame I_(i), so that in this step an arbitrary depth can be specified for x_(k). In the method, the average depth d_(k) of the key frame F_(k) is used to initialize X, that is X=d_(k)K⁻¹x_(k) ^(h), and X is optimized according to a formula of

$\begin{matrix} {{\underset{X}{argmin}{{{\pi \left( {{KR}_{k}\left( {X - p_{k}} \right)} \right)} - x_{k}}}_{2}^{2}} + {{{{\pi \left( {{KR}_{i}\left( {X - p_{i}} \right)} \right)} - x_{i}}}_{2}^{2}.}} & (9) \end{matrix}$

Once there is enough motion parallax, the depth error may affect camera tracking. It is best to perform bundle adjustment to the local map at the time when each new frame arrives, but this cannot achieve real-time performance. It is therefore possible to modify the optimization process so as to optimize only the three-dimensional point or the camera orientation at a time, and these two steps are carried out in turn. This strategy is very effective in practical applications, and can significantly reduce the computing load to achieve real-time, even in mobile devices.

Optimizing the three-dimensional point under the premise of a fixed camera orientation is relatively simple. Each three-dimensional point X_(i) can be individually optimized by the minimization formula

$\begin{matrix} {{\underset{X_{i}}{argmin}{\sum\limits_{j \in V_{X_{i}}}{{{\pi \left( {K\left( {R_{j}\left( {X_{i} - p_{j}} \right)} \right)} \right)} - x_{ij}}}_{\delta_{x}}}},} & (10) \end{matrix}$

where V_(x) _(i) is a set of frame number indices of the key frame and a frame where the three-dimensional point X_(i) is observed in a sliding window.

The orientation of each camera i is optimized separately using equation

$\begin{matrix} {{\underset{C_{i}}{argmin}{\sum\limits_{j \in V_{i}}{{{\pi \left( {K\left( {R_{i}\left( {X_{j} - p_{i}} \right)} \right)} \right)} - x_{ij}}}_{\delta_{x}}}},} & (11) \end{matrix}$

where V_(i) is a set of point indices of three-dimensional points visible in the frame i. However, any feature tracking method may fail due to the severe blur caused by fast camera motion. These blurred frames lack the constraints of reliable camera orientation estimation. In view of that the fast camera motion typically only occurs occasionally and does not last long, adjacent frames can be used to constrain the blurred frames. Specifically, we draw on the idea of VI-SLAM, such as (A. I. Mourikis and S. I. Roumeliotis. A multi-state constraint kalman filter for vision-aided inertial navigation. In IEEE International Conference on Robotics and Automation, pages 3565-3572. IEEE, 2007), (J. A. Hesch, D. G. Kottas, S. L. Bowman, and S. I. Roumeliotis. Consistency analysis and improvement of vision-aided inertial navigation. IEEE Transactions on Robotics, 30 (1): 158-176, 2014) and (S. Leutenegger, S. Lynen, M. Bosse, R. Siegwart, and P. Furgale. Keyframe-based visual-inertial odometry using nonlinear optimization. The International Journal of Robotics Research, 34 (3):314-334, 2015). Assume that there is already IMU measurement data in the local coordinate system (including a linear acceleration â and a rotation speed {circumflex over (ω)}) and a camera attitude is the same as that of the IMU, the state of the camera motion can be extended to s=[q^(T) p^(T) v^(T) b_(a) ^(T) b_(ω) ^(T)]^(T) where v is the linear velocity in the global coordinate system, and b_(a) and b_(ω)are respectively offsets of the linear acceleration and the rotation speed with time. The real linear acceleration is a=â−b_(a)+n_(a)(13) and the rotation speed is ω={circumflex over (ω)}−b_(ω)+n_(ω)(14), where n_(a)˜N(0, σ_(n) _(a) ²I), n_(ω)˜N(0,σ_(n) _(a) ²I) is the Gaussian noise of the inertial measurement data, and I is a 3×3 identity matrix. In the experiment, it is generally set that σ_(n) _(a) =0.01 m/(s²√{square root over (Hz)}), σ_(n) _(a) =0.01 rad/(s√{square root over (Hz)}) (if the actual IMU measurement data is provided, setting σ_(n) _(a) =0.001 rad/(s√{square root over (Hz)}). The continuous-time motion model (A. B. Chatfield. Fundamentals of high accuracy inertial navigation. AIAA, 1997) describes the time evolution of states:

$\begin{matrix} {{\overset{.}{q} = {\frac{1}{2}{\Omega (\omega)}q}},{\overset{.}{p} = v},{\overset{.}{v} = {R^{T}a}},{{\overset{.}{b}}_{a} = w_{a}},{{\overset{.}{b}}_{\omega} = w_{\omega}},} & (15) \end{matrix}$

where

${\Omega (\omega)} = \begin{bmatrix} {- \lbrack\omega\rbrack_{x}} & \omega \\ {- \omega^{T}} & 0 \end{bmatrix}$

and [⋅]_(x) represents a skew symmetric matrix. b_(a) and {dot over (b)}_(ω) are respectively random walk processes represented by the Gaussian white noise model w_(a)˜N(0, σ_(w) _(a) ²I) and w_(ω)˜N(0, σ_(w) _(ω) ²I) . In the experiment, it is set that σ_(w) _(a) =0.001 m/(s³√{square root over (Hz)}), σ_(w) _(a) =0.000 rad/(s²√{square root over (Hz)}. For the discrete-time state, an integration method proposed by (N. Trawny and S. I. Roumeliotis. Indirect kalman filter for 3D attitude estimation. Technical Report 2005-002, University of Minnesota, Dept. of Comp. Sci. & Eng., March 2005) is used to transmit the quaternion:

$\begin{matrix} {{q_{i + 1} = {{q_{\Delta}\left( {\omega_{i},t_{\Delta_{i}}} \right)} \otimes q_{i}}},{where}} & (17) \\ {{q_{\Delta}\left( {\omega,t_{\Delta}} \right)} = \left\{ {\begin{matrix} \begin{bmatrix} {\frac{\omega}{\omega }{\sin \left( {\frac{\omega }{2}t_{\Delta}} \right)}} \\ {\cos \left( {\frac{\omega }{2}t_{\Delta}} \right)} \end{bmatrix} & {{{if}\mspace{14mu} {\omega }} > ɛ_{\omega}} \\ \begin{bmatrix} {\frac{\omega }{2}t_{\Delta}} \\ 1 \end{bmatrix} & {otherwise} \end{matrix},} \right.} & (18) \end{matrix}$

where t_(Δ) _(i) is a time interval between two adjacent key frames i and i+1, ⊗ is a quaternion multiplication operator, and ε_(ω)a small value that prevents zero division. In the experiment, it is set that ε_(ω)=0.00001 rad/s. The real value of q_(Δ)(ω_(i), t_(Δ) _(i) ) is approximated by the formula

$\begin{matrix} {{{q_{\Delta}\left( {\omega_{i},t_{\Delta_{i}}} \right)} \approx {\begin{bmatrix} \frac{\overset{\sim}{\theta}}{2} \\ 1 \end{bmatrix} \otimes {q_{\Delta}\left( {{{\hat{\omega}}_{i} - b_{\omega_{i}}},t_{\Delta_{i}}} \right)}}},} & (19) \end{matrix}$

where {tilde over (θ)}is a 3×1 error vector. By substituting formula (14) into formula (19), it can be obtained that:

{tilde over (θ)}≈2[q _(Δ)({circumflex over (ω)}_(i) −b _(ω) _(i) +n ₁₀₇ ,t _(Δ) ₁ )⊗q _(Δ) ⁻¹({circumflex over (ω)}_(i) −b _(ω) _(i) ,t _(Δ) _(i) )]_(1:3)≈2G _(ω) n _(ω)  (20),

where G_(ω)is a Jacobian matrix associated with noise By substituting the formula (19) into the formula (17), it can be obtained that:

{tilde over (θ)}≈2[q _(i+1) ⊗q _(i) ⁻¹ ⊗q _(Δ) ⁻¹({circumflex over (ω)}_(i) −b _(ω) _(i) ,t _(Δ) _(i) ]_(1:3)  (21).

Combining (20) and (21), the cost function and covariance of the rotation component are respectively defined as:

e _(q)(q _(i) ,q _(i+1) ,b _(ω) _(i) )=[q _(i+1) ⊗q _(i) ⁻¹ ,⊗q _(Δ) ⁻¹({circumflex over (ω)}_(i) −b _(ω) _(i) ,t _(Δ) _(i) )]_(1:3), Σ_(q)=σ_(n) _(ω) ² ,G _(ω) G _(ω) ^(T)   (22).

The derivation for other components is relatively simple. The transitivity formulas of the discrete-time states are as follows:

p _(i+1) =p _(i) +v _(i) t ₆₆ _(i) +R _(i) ^(T) p _(Δ)(a _(i) ,t _(Δ) _(i) )

v _(i+1) =v _(i) +R _(i) ^(T) v _(Δ)(a _(i) ,t _(Δ) _(i) )

b _(a) _(i+1) +b _(a) _(i) +w _(a) t _(Δ) _(t)

b _(ω) _(i+1) =b _(ω) _(i) +w _(ω) t _(Δ) _(t)   (23)

where

$\begin{matrix} {{{p_{\Delta}\left( {a,t_{\Delta}} \right)} = {\frac{1}{2}{at}_{\Delta}^{2}}},{{v_{\Delta}\left( {a,t_{\Delta}} \right)} = {{at}_{\Delta}.}}} & (24) \end{matrix}$

The error function and the corresponding covariance are:

$\begin{matrix} {{{e_{p}\left( {q_{i},p_{i},p_{i + 1},v_{i},b_{a_{i}}} \right)} = {{R_{i}\left( {p_{i + 1} - p_{i} - {v_{i}t_{\Delta_{i}}}} \right)} - {p_{\Delta}\left( {{{\hat{a}}_{i} - b_{a_{i}}},t_{\Delta_{i}}} \right)}}}{{e_{v}\left( {q_{i},v_{i},v_{i + 1},v_{i},b_{a_{i}}} \right)} = {{R_{i}\left( {v_{i + 1} - v_{i}} \right)} - {v_{\Delta}\left( {{{\hat{a}}_{i} - b_{a_{i}}},t_{\Delta_{i}}} \right)}}}\mspace{20mu} {{e_{b_{a}}\left( {b_{a_{i}},b_{a_{i + 1}}} \right)} = {b_{a_{i + 1}} - b_{a_{i}}}}\mspace{20mu} {{e_{b_{\omega}}\left( {b_{\omega_{i}},b_{\omega_{i + 1}}} \right)} = {b_{\omega_{i + 1}} - b_{\omega_{i}}}}\mspace{20mu} {\Sigma_{p} = {\frac{1}{4}\sigma_{n_{a}}^{2}t_{\Delta_{i}}^{4}I}}\mspace{20mu} {\Sigma_{v} = {\sigma_{n_{a}}^{2}t_{\Delta_{i}}^{2}I}}\mspace{20mu} {\Sigma_{b_{a}} = {\sigma_{w_{a}}^{2}t_{\Delta_{i}}^{2}I}}\mspace{20mu} {\Sigma_{b_{\omega}} = {\sigma_{w_{\omega}}^{2}t_{\Delta_{i}}^{2}{I.}}}} & (25) \end{matrix}$

Based on these correlation constraints between adjacent frames defined by (22) and (25), the energy function of all motion states s₁ . . . s₁ in the sliding window can be defined as:

$\begin{matrix} {{{\underset{s_{1}\ldots \mspace{14mu} s_{l}}{\arg \; \min}{\sum\limits_{i = 1}^{l}{\sum\limits_{j \in V_{C_{i}}}{{{\pi \left( {K\left( {R_{i}\left( {X - p_{i}} \right)} \right)} \right)} - x_{ij}}}}}} + {\sum\limits_{i = 1}^{l - 1}{{e_{q}\left( {q_{i},q_{i + 1},b_{\omega_{i}}} \right)}}_{\Sigma_{q}}^{2}} + {\sum\limits_{i = 1}^{l - 1}{{e_{p}\left( {q_{i},p_{i},p_{i + 1},v_{i},b_{a_{i}}} \right)}}_{\Sigma_{p}}^{2}} + {\sum\limits_{i = 1}^{l - 1}{{e_{v}\left( {q_{i},v_{i},v_{i + 1},v_{i},b_{a_{i}}} \right)}}_{\Sigma_{v}}^{2}} + {\sum\limits_{i = 1}^{l - 1}{{e_{b_{a}}\left( {b_{a_{i}},b_{a_{i + 1}}} \right)}}_{\Sigma_{b_{a}}}^{2}} + {\sum\limits_{i = 1}^{l - 1}{{e_{b_{\omega}}\left( {b_{\omega_{i}},b_{\omega_{i + 1}}} \right)}}_{\Sigma_{b_{\omega}}}^{2}}},} & (26) \end{matrix}$

where l is a size of the sliding window and ∥e∥_(Σ) ²=e^(T)Σ⁻¹e is a square of a Mahalanobis distance.

The above derivatives assume that inertial measurement data â_(i) and {circumflex over (ω)}_(i) are known, but actually there may be no IMU sensor. For the linear acceleration, it can be set to be â_(i)=0, because there is almost no sudden jump in the AR application. However, for the rotation speed, since the user often needs to turn his or her head to view the entire AR effect, it cannot be set to be {circumflex over (ω)}_(i)=0. Therefore, feature matching is used to align the continuous SBI to obtain a best fitting:

$\begin{matrix} {{{\hat{\omega}}_{i} = {\underset{\omega}{\arg \; \min}\left( {{\sum\limits_{x \in \Omega}{{{{\overset{\sim}{I}}_{i}(x)} - {{\overset{\sim}{I}}_{i + 1}\left( {\pi \left( {{{KR}_{\Delta}\left( {\omega,t_{\Delta_{i}}} \right)}K^{- 1}x^{h}} \right)} \right)}}}_{\delta_{I}}} + {\sum\limits_{{({x_{i},x_{i + 1}})} \in M_{i,{j + 1}}}{\frac{1}{\delta_{X}}{{{\pi \left( {{{KR}_{\Delta}\left( {\omega,t_{\Delta_{i}}} \right)}K^{- 1}x_{i}^{h}} \right)} - x_{i + 1}}}_{2}^{2}}}} \right)}},} & (27) \end{matrix}$

where R_(Δ)(ω,t_(Δ)) is a rotation matrix of formula (18), and M_(i,i+1) is a set of feature matchings between images i and i+1. δ_(X) controls a weight of a feature matching item and is set to be 10 in the experiment. The Gauss Newton algorithm is used to solve the formula (27).

The above optimization framework can be easily extended to the case of combining real IMU data, and the main difference lies in that there may be multiple IMU measurement data between two adjacent frames. Moreover, there is always some differences between the attitude of the camera and the attitude of the real IMU sensor, which cannot be simply considered to be consistent (the relative attitude of the camera and the real IMU sensor can be obtained by prior calibration). The set of IMU measurement data between frames i and i+1 is denoted as {(aâ_(ij), {circumflex over (ω)}_(ij), t_(ij))|j=1 . . . n_(i)}, where â_(ij), {circumflex over (ω)}_(ij) and t_(ij) are the j_(th) linear acceleration, the rotation speed and the time stamp, respectively. The pre-integration technology in (T. Lupton and S. Sukkarieh. Visual-inertial-aided navigation for high-dynamic motion in built environments without initial conditions. IEEE Transactions on Robotics, 28(1):61-76, 2012) and (C. Forster, L. Carlone, F. Dellaert, and D. Scaramuzza. IMU preintegration on manifold for efficient visual-inertial maximum-a-posteriori estimation. In Robotics: Science and Systems, 2015) can be used to pre-integrate these IMU measurement data, and the components in (18) and (24) are replaced with the following formulas:

$\mspace{20mu} {{q_{\Delta}\left( \left\{ \left( {{{\hat{\omega}}_{ij} - b_{\omega_{i}}},t_{ij}} \right) \right\} \right)} \approx {{q_{\Delta}\left( {{{\hat{\omega}}_{ij} - {\hat{b}}_{\omega_{i}}},t_{ij}} \right)} + {\frac{\partial q_{\Delta}}{\partial b_{\omega}}\left( {b_{\omega} - {\hat{b}}_{\omega}} \right)}}}$ ${{p_{\Delta}\left( \left\{ \left( {{{\hat{a}}_{ij} - b_{a_{i}}},{{\hat{\omega}}_{ij} - b_{\omega_{i}}},t_{ij}} \right) \right\} \right)} \approx {{p_{\Delta}\left( \left\{ \left( {{{\hat{a}}_{ij} - {\hat{b}}_{a_{i}}},{{\hat{\omega}}_{ij} - {\hat{b}}_{\omega_{i}}},t_{ij}} \right) \right\} \right)} + {\frac{\partial p_{\Delta}}{\partial b_{a}}\left( {b_{a} - {\hat{b}}_{a}}\; \right)} + {\frac{\partial p_{\Delta}}{\partial b_{\omega}}\left( {b_{\omega} - {\hat{b}}_{\omega}} \right)}}},{{v_{\Delta}\left( \left\{ \left( {{{\hat{a}}_{ij} - b_{a_{i}}},{{\hat{\omega}}_{ij} - b_{\omega_{i}}},t_{ij}} \right) \right\} \right)} \approx {{v_{\Delta}\left( \left\{ \left( {{{\hat{a}}_{ij} - {\hat{b}}_{a_{i}}},{{\hat{\omega}}_{ij} - {\hat{b}}_{\omega_{i}}},t_{ij}} \right) \right\} \right)} + {\frac{\partial v_{\Delta}}{\partial b_{a}}\left( {b_{a} - {\hat{b}}_{a}}\; \right)} + {\frac{\partial v_{\Delta}}{\partial b_{\omega}}\left( {b_{\omega} - {\hat{b}}_{\omega}} \right)}}}$

where {circumflex over (b)}_(a) _(i) and {circumflex over (b)}_(ω) _(i) are states of b_(a) _(i) , and b_(ω) _(i) when the pre-integration are performed. The Jacobian matrix is estimated in this state and the calculation only needs to be performed once. It is assumed here that the gravity component has been excluded, so â_(ij) only includes the acceleration generated by the motion. The sensor fusion algorithm in (N. Trawny and S. I. Roumeliotis. Indirect kalman filter for 3D attitude estimation. Technical Report 2005-002, University of Minnesota, Dept. of Comp. Sci. & Eng., March 2005) can be adopted to process original data obtained from accelerometers and gyroscopes so as to obtain the required data. In the experiment, the used IMU measurement data was from the filtered data in the iPhone 6.

3.2. When the result is medium (not good), there are still some matched features, most of which are still fully constrained points, and thus, a set of local homography matrices H_(k→i) ^(L) can be estimated and a feature that has failed to be tracked is re-matched.

If the quality of the tracking of the global homography matrix and the specific three-dimensional plane homography matrix is not good, the obtained matching can be further used to estimate the local homography matrix. The present method uses a simple extraction strategy of multiple homography matrices to estimate a set of local homography matrices. Specifically, RANSAC method is used for M_(k,i), and a best local homography matrix is estimated based on the number of the interior points satisfying the homography transformation and denoted as H_(k→i) ^(L) ¹ . The interior point satisfying H_(k→i) ^(L) ¹ is removed from M_(k,i), and then the same operation is performed on the remaining matchings to obtain H_(k→i) ^(L) ² . The above steps are repeated until the number of the remaining matchings is smaller than a certain number.

For a feature point which is not fully constrained and has no reliable three-dimensional position, π(H_(k→i) ^(G)x_(k) ^(h)) is first directly used as the search position. If the tracking result is not good, a set of local homography matrices H_(k→i) ^(L) is estimated according to the matching correspondences. For the unmatched features, trying is performed one by one in the search position {H_(k→i) ^(L) ^(j) x_(k)|H_(k→i) ^(L) ^(j) ∈H_(k→i) ^(L)}. For each search position, its range is a r×r square area centered on the search position. For the points fully and un-fully constrained, r is 10 and 30 respectively.

The representation method of the multiple homography matrices in the present method can not only predict a good starting point but also correct perspective distortion. A homography matrix H_(k→i) is given, and the local block near x_(k) in the key frame F_(k) is aligned to the current frame I_(i) through the formula κ(y)=I_(k)(H_(k→i) ⁻¹(π(H_(k→i)x_(k) ^(h)+y^(h)))) (7), y^(h), where κ(y) is an image gray scale for an offset of y relative to a center of the block. y∈(−W/2, W/2)×(−W/2, W/2), where W is a dimension of the image block and is set to be 8 in the experiment. For points belonging to the three-dimensional plane P_(j), the plane homography matrix H_(k→i) ^(P) ^(j) obtained by the formula (2) is used for image block alignment. For other points, similar to the prediction of the search scope, the global homography matrix H_(k→i) ^(G) is tried first, and if the tracking result is poor, the local homography matrix in the set H_(k→i) ^(L) is tried one by one for features a feature that have failed to be tracked. Considering that a significant perspective change between adjacent frames is rare, it is not necessary to recalculate formula (7) for each input frame. The change of four vertices of each aligned image block may be calculated and formula (7) is recalculated only when the change exceeds a certain threshold.

Given a precise search range and an undistorted image block, the correspondence in the current frame can be easily tracked and obtained. Similar to PTAM, a value of a zero mean SSD is calculated for a FAST corner point extracted within a search area, and the best one is selected when the offset is smaller than a certain threshold.

In the present method, since a sufficient feature matching can generally be obtained by a global homography matrix and a specific plane homography matrix, it is rarely necessary to solve a local homography matrix. Only when there is a large parallax between the frames F_(k) and I_(i), and there are multiple different planes in the overlapping area, it is necessary to additionally try each local homography matrix for those abnormal points. As shown in FIG. 3, 37 pairs of matchings can be obtained by the global and specific plane homography matrices, and additional 153 pairs of matchings can be obtained by the local homography matrix.

3.3. When the result is poor, the relocating program introduced in the paper (G. Klein and D. W. Murray. Parallel tracking and mapping for small AR workspaces. In 6th IEEE/ACM International Symposium on Mixed and Augmented Reality, pages 225-234, 2007) is triggered. Once the relocating is successful, the relocated key frame is used for global homography matrix tracking, and then the feature tracking is performed again.

4. Waking up the background thread if a new key frame is selected.

4.1. A new key frame and a newly triangulated three-dimensional point are added for extending the global map, and a local bundle adjustment is adopted for optimization. After that, the existing three-dimensional plane is extended and the newly added three-dimensional point is given to the existing plane, or a new three-dimensional plane is extracted from the newly added three-dimensional point.

Similar to the paper (G. Klein and D. W. Murray. Parallel tracking and mapping for small AR workspaces. In 6th IEEE/ACM International Symposium on Mixed and Augmented Reality, pages 225-234, 2007), the present method uses local and global bundle adjustment methods in the background to optimize the map and camera orientation of the key frame. For each three-dimensional point in the new key frame F_(k), the maximum ray angle with respect to the existing key frame is first calculated using formula (8). If max, α(i,k)≥δ_(α)and the three-dimensional coordinates are successfully triangulated, the point is marked as fully constrained. The existing three-dimensional plane visible in frame F_(k) is then extended with a new and fully constrained three-dimensional point X belonging to the plane. In order to determine whether a three-dimensional point X belongs to the plane P, it is only needed to check the distance |n^(T)X+d| from the point to the plane. However, satisfying the plane equation is only a necessary condition rather than a sufficient condition, due to the fact that the plane equation does not contain plane boundary information. Suppose that the points belonging to the same plane are close to each other in space, only a point having its adjacent point on the plane is checked when checking the point-to-plane distance. The neighborhood relationship is defined by Delaunay triangulation of the two-dimensional feature in the key frame F_(k). When a three-dimensional point satisfies the following three conditions, the point is added to the set V_(P) of the plane P: 1) X is not assigned to any plane, 2) X is connected with another point in V_(P), and 3) |n^(T)X+d|<δ_(P). In the experiment, setting δ_(P)=0.01d_(k), where d_(k) is an average depth of the key frame F_(k).

RANSAC algorithm (M. A. Fischler and R. C. Bolles. Random sample consensus: A paradigm for model fitting with applications to image analysis and automated cartography. Commun. ACM, 24(6): 381-395, 1981) is adopted to extract a new three-dimensional plane. In each iteration of the RANSAC, three connected points are randomly sampled to initialize a set of three-dimensional interior points V_(P). It is assumed that the three-dimensional plane P is generated by V_(P). Then, an inner loop which iteratively adjusts plane extension and optimization begins. In each internal iteration, the distance from those three-dimensional points X, which are connected to points in the set V_(P), to the plane is checked. If |n^(T)X+d|<δ_(P), X is added to V_(P). After that, the plane is optimized by the minimization formula for all points in V_(P):

$\begin{matrix} {{\underset{P}{\arg \; \min}{\sum\limits_{X \in V_{P}}\left( {{n^{T}X} + d} \right)^{2}}},} & (28) \end{matrix}$

and an attempt to further extend V_(P) is made. When |V_(P)|<30, P and V_(P) are discarded. In order to avoid marking one point to multiple planes, the extracted planes are sorted in a descending order according to the number of the relevant three-dimensional points after each RANSAC operation. The set of the sorted interior points is denoted as {V_(P) _(i) |i=1 . . . N_(P)}. Starting from the first one, for any V_(P) _(i) , if a three-dimensional point X∈V_(P) _(i) also exists in the previous set of the plane points V_(P) _(j) (j<i) , this point is to be removed from V_(P) _(i) . FIG. 4 is illustrated as the extracted three-dimensional plane.

By adding a point-to-plane constraint, parameters of the three-dimensional plane can be optimized in a global bundle adjustment. The present method uses a LM (Levenberg Marquart) algorithm to minimize the following energy function so as to jointly optimize all camera orientations, three-dimensional points and three-dimensional planes of the key frames:

$\begin{matrix} {{\underset{C,X,P}{\arg \; \min}{\sum\limits_{i = 1}^{N_{F}}{\sum\limits_{j \in V_{i}}{{{\pi \left( {K\left( {R_{i}\left( {X_{j} - p_{i}} \right)} \right)} \right)} - x_{ij}}}_{\delta_{x}}}}} + {\sum\limits_{i = 1}^{N_{P}}{\sum\limits_{j \in V_{P_{i}}}{{{{n_{i}^{T}X_{j}} + d_{j}}}_{\delta_{P}}.}}}} & (29) \end{matrix}$

It should be noted that other functions of the present method are also solved by the LM algorithm except (3), (5) and (27).

4.2. Detecting a loop by matching features of a new key frame with those of an existing key frame.

For the loop detection, the same algorithm as the relocation is used to find feature matching of the new key frame with the previous frame, and then a closed circulation loop is obtained through the global bundle adjustment.

4.3. Performing a bundle adjustment on the global map.

In the experiment, effects of the method proposed by the present disclosure in four different settings are first compared (FIG. 5): 1) a case where there is no a priori motion constraint; 2) a case where it is directly set that â_(i)=0, {circumflex over (ω)}_(i)=0; 3) a case where the rotation speed is estimated; and 4) a case where the actual IMU data is incorporated. The results indicate that in the cases of 1) and 2), the present method is similar to results of other methods, in both of which the results of tracking failure frequently occur during strong rotation and rapid motion, as shown in FIGS. 5(a) and (b). In the case of 3), the present method can reliably recover the camera orientation even during fast motion and severe blur, as shown in FIG. 5(c). When the actual IMU data is incorporated, the robustness is further improved, as shown in FIG. 5(d). In addition, the results of ORB-SLAM, PTAM, and LSD-SLAM are shown in FIGS. 5(e), (f), and (g), in which severe drift occurs due to strong rotation, rapid motion and severe blurring. Then, the effect of the present method was compared with those of other methods in the outdoor large scene. The result of the present method again demonstrates the superiority of the present method. In the absence of actual IMU data, the result of the present method can still recover and obtain a reliable camera orientation, and when there is actual IMU data, the effect is more reliable.

In addition, we have applied this method in an application on a mobile terminal and tested it. When the user is using the application, a scene is shot with a mobile phone, and then a plurality of three-dimensional virtual furniture models are inserted into the room for viewing the effect of the furniture placement. We could not know the actual size of the three-dimensional structure recovered by the Monocular Simultaneous Localization and Mapping (SLAM) method. In order to ensure that the three-dimensional furniture model has the right size when being inserted, a method is needed for accurate estimation. However, it may be difficult to estimate the exact size in time based on the noisy IMU data provided by the mobile phone. It is more reliable to use a calibration of a known size. In the application, the user needs to use a sheet of A4 paper, aiming a center of a camera at the paper at the beginning of the application. Our augmented reality system automatically uses camera orientation estimates to automatically detect four sides of the paper. After initialization, the user can download different three-dimensional models in a list and put them into the scene. 

1. A method for simultaneous localization and mapping, comprising steps of: 1) a foreground thread processing a video stream in real time, and extracting a feature point for any current frame I_(i); 2) tracking a set of global homography matrices H_(i) ^(G); 3) using a global homography matrix and a specific plane homography matrix to track a three-dimensional point so as to obtain a set of 3D-2D correspondences required by a camera attitude estimation; and 4) evaluating quality of the tracking according to a number of the tracked feature points and classifying the quality into good, medium and poor; 4.1) when the quality of the tracking is good, performing extension and optimization of a local map, and then determining whether to select a new key frame; 4.2) when the quality of the tracking is medium, estimating a set of local homography matrices H_(k→i) ^(L), and re-matching a feature that has failed to be tracked; and 4.3) when the quality of the tracking is poor, triggering a relocating program, and once the relocating is successful, performing a global homography matrix tracking using a key frame that is relocated, and then performing tracking of features again, wherein in step 4.1), when it is determined to select a new key frame, the selected new key frame wakes up a background thread for global optimization, the new key frame and a new triangulated three-dimensional point is added for extending a global map, and a local bundle adjustment is adopted for optimization; then an existing three-dimensional plane is extended, the added new three-dimensional point is given to the existing plane, or a new three-dimensional plane is extracted from the added new three-dimensional point; subsequently, a loop is detected by matching the new key frame with an existing key frame; and finally, a global bundle adjustment is performed on the global map.
 2. The method for simultaneous localization and mapping according to claim 1, wherein the step 2) comprises: for two adjacent frames of images, estimating a global homography transformation from a previous frame of image I_(i−l) to a current frame of image I_(i) by directly aligning the previous frame of image with the current frame of image as: ${H_{{({i - 1})}\rightarrow i}^{G} = {\underset{H}{\arg \; \min}{\sum\limits_{X \in \Omega}{{{{\overset{\sim}{I}}_{i - 1}(x)} - {{\overset{\sim}{I}}_{i}\left( {\pi\left( {\overset{\sim}{H}x^{h}} \right)} \right)}}}_{\delta_{I}}}}},$ where Ĩ_(t-1) and Ĩ_(i−1) are small blurred images of I_(i−1) and I_(i) respectively, Ω is a small image domain, a superscript h indicates that a vector is a homogeneous coordinate, a tilde in {tilde over (H)} indicates that a homography matrix H is converted from a source image space to a small image space, δ_(I) and ∥⋅∥_(δ) represents a Huber function; and a calculation method is expressed as the following formula: $\begin{matrix} {{e}_{\delta} = \left\{ {\begin{matrix} \frac{{e}_{2}^{2}}{2\delta} & {{{if}\mspace{14mu} {e}_{2}} \leq \delta} \\ {{e}_{2} - \frac{\delta}{2}} & {{otherwise}.} \end{matrix};} \right.} & (4) \end{matrix}$ for each key frame F_(k), which has an initial solution to the global homography matrix H_(k→(i−1)) ^(G) of I_(i−1), using a set of feature matchings (denoted as M_(k,i−1)={(x_(k),x_(i−1))}) between frames F_(k) and I_(i−1) to determine a set of key frames which has an overlap with I_(i−1) and is denoted as K_(i−1), selecting five key frames satisfying that |M_(k,i−1)|>20 and |M_(k,i−1)| an is maximized, optimizing initial solution H_(k→(i−1)) ^(G) for each key frame F_(k) in K_(i−1): ${H_{k\rightarrow{({i - 1})}}^{G} = {\underset{H}{\arg \; \min}{\sum\limits_{X \in \Omega}\left( {{{{{\overset{\sim}{F}}_{k}(x)} - {{\overset{\sim}{I}}_{i - 1}\left( {\pi \left( {\overset{\sim}{H}x^{h}} \right)} \right)}}}_{\delta_{I}} + {\sum\limits_{{({x_{k},x_{i - 1}})} \in M_{k,{i - 1}}}{{{\pi \left( {\overset{\sim}{H}x_{k}^{h}} \right)} - x_{i - 1}}}_{\delta_{x}}}} \right)}}},\delta_{x}$ and obtaining the global homography transformation H_(k→1) ^(G)=H_((i−1)→i) ^(G)H_(k→(i−1)) ^(G) from the key frame F_(k) to the current frame I_(i) through transitivity.
 3. The method for simultaneous localization and mapping according to claim 1, wherein the step 3) comprises: for a feature point which is fully constrained and has a reliable three-dimensional position, determining a search position by predicting a camera orientation of a current frame C_(i), predicting C_(i) using the global homography matrix H_(i) ^(G), denoting a two-dimensional feature point of a frame F_(k)∈K_(i−1) as x_(k) and denoting a corresponding three-dimensional point as X_(k), obtaining a three-dimensional to two-dimensional correspondence (X_(k),x_(i)) through a formula x_(i)=π(H_(k→i) ^(G)x_(k) ^(h)), in which a set of all the three-dimensional to two-dimensional correspondences is denoted as V_(C) _(i) ={(X_(j),x_(j))}, and predicting a camera position by solving $\begin{matrix} {{\underset{C_{i}}{\arg \; \min}{\sum\limits_{{({X_{j},x_{j}})} \in V_{C_{i}}}{{{{\pi \left( {K\left( {R_{i}\left( {X_{j} - p_{i}} \right)} \right)} \right)} - x_{j}}}_{\delta_{x}}\mspace{14mu} R_{i}}}};} & (6) \end{matrix}$ and for a feature point which is not fully constrained and has no reliable three-dimensional position, first of all, directly using π(H_(k→i) ^(G)x_(k) ^(h)) as the search position, and obtaining a set of local homography matrices according to estimation of matched correspondence if the quality of the tracking is evaluated as medium, while for an unmatched feature, trying one by one in the search position {H_(k→i) ^(L) ^(j) x_(k)|H_(k→i) ^(L) ^(j) ∈H_(k→i) ^(L)}, in which for each search position, its range is a r×r square area centered on the search position and r is 10 and 30 respectively for points fully constrained and points not fully constrained; and given a homography matrix H_(k→i), calculating a change of four vertices of each aligned image block, aligning a local block near x_(k) in the key frame F_(k) to the current frame I_(i) through a formula κ(y)=I_(k)(H_(k→i) ⁻¹(πH_(k→i)x_(k) ^(h)+y^(h)))) (7) only when the change exceeds a certain thresholdy^(h), wherein κ(y) is an image gray scale for an offset of y relative to a center of the block, and y∈(−W/2, W/2)×(−W/2, W/2), where W is a dimension of the image block; and calculating a value of a zero-mean SSD for a FAST corner point extracted within a search area, and selecting one FAST corner which has the minimum zero-mean SSD when the offset is smaller than a certain threshold.
 4. The method for simultaneous localization and mapping according to claim 1, wherein said performing extension and optimization of the local map in step 4.1) comprises: for one two-dimensional feature x_(k) of a frame F_(k), calculating a ray angle according to a formula of $\begin{matrix} {{\alpha \left( {i,k} \right)} = {{acos}\left( \frac{r_{k}^{T}r_{i}}{{r_{k}} \cdot {r_{i}}} \right)}} & (8) \end{matrix}$ once a corresponding feature x_(i) is found in a frame I_(i), where r_(j)=R_(j) ^(T)K⁻¹x_(j) ^(h); if α(i,k)≥δ_(α), obtaining a three-dimensional position X using a triangulation algorithm, and otherwise, specifying an average depth d_(k) of the key frame F_(k) for x_(k) to initialize X, X=d_(k)K⁻¹x_(k) ^(h), and optimizing X according to a formula of arg min∥π(KR_(k)(X−p_(k)))−x_(k)∥₂ ²+∥π(KR_(i)(X−p_(i)))−x_(i)∥₂ ²(9)δ_(α)ρ_(i); selecting a local window, fixing a camera orientation first, individually optimizing each three-dimensional point position X_(i) by solving $\begin{matrix} {{\underset{X_{i}}{\arg \; \min}{\sum\limits_{j \in V_{X_{i}}}{{{\pi \left( {K\left( {R_{j}\left( {X_{i} - p_{i}} \right)} \right)} \right)} - x_{ij}}}_{\delta_{x}}}},} & (10) \end{matrix}$ where V_(x) _(i) , is a set of frame number indices of the key frame and a frame where the three-dimensional point X_(i) is observed in a sliding window, and then fixing the three-dimensional point position and optimizing all camera orientations in the local window.
 5. The method for simultaneous localization and mapping according to claim 4, wherein said fixing the three-dimensional point position and optimizing all camera orientations in the local window comprises: assuming that there is already a linear acceleration â and a rotation speed {circumflex over (ω)} measured in a local coordinate system, and a real linear acceleration is a=aâ−b_(a)+n_(a), a real rotation speed is ω={circumflex over (ω)}−b_(ω)+n_(ω), n_(a)˜N(0, σ_(n) _(a) ²I), n_(ω)˜N(0,σ_(n) _(ω) ²I) are Gaussian noise of inertial measurement data, I is a 3×3 identity matrix, b_(a) and b_(ω) are respectively offsets of the linear acceleration and the rotation speed with time, extending a state of a camera motion to be: s=[q^(T) p^(T) v^(T) b_(a) ^(T) b_(ω) ^(T)]^(T), where v is a linear velocity in a global coordinate system, a continuous-time motion equation of all states is: $\begin{matrix} {{\overset{.}{q} = {\frac{1}{2}{\Omega (\omega)}q}},{\overset{.}{p} = v},{\overset{.}{v} = {R^{T}a}},{{\overset{.}{b}}_{a} = w_{a}},{{\overset{.}{b}}_{\omega} = w_{\omega}},} & (15) \end{matrix}$ where ${{\Omega (\omega)} = \begin{bmatrix} {- \lbrack\omega\rbrack_{x}} & \omega \\ {- \omega^{T}} & 0 \end{bmatrix}},$ [⋅] represents a skew symmetric matrix, w_(a)˜N(0,σ_(w) _(a) ²I) and w_(ω)˜N(0,σ_(w) _(ω) ²I) represent random walk processes, and a discrete-time motion equation of a rotational component is: q _(i+1) =q _(Δ)(ω_(i) ,t _(Δ) _(i) )⊗q _(i) where   (17), $\begin{matrix} {{q_{\Delta}\left( {\omega,t_{\Delta}} \right)} = \left\{ {\begin{matrix} \begin{bmatrix} {\frac{\omega}{\omega }{\sin \left( {\frac{\omega }{2}t_{\Delta}} \right)}} \\ {\cos \left( {\frac{\omega }{2}t_{\Delta}} \right)} \end{bmatrix} & {{{if}\mspace{14mu} {\omega }} > ɛ_{\omega}} \\ \begin{bmatrix} {\frac{\omega }{2}t_{\Delta}} \\ 1 \end{bmatrix} & {otherwise} \end{matrix},} \right.} & (18) \end{matrix}$ frames i and i+1, ⊗ is a quaternion multiplication operator, ε_(ω)is a small value that prevents zero division; and approximating a real value of q_(Δ)(ω_(i),t_(Δ) _(i) ) by a formula of $\begin{matrix} {{{q_{\Delta}\left( {\omega_{i},t_{\Delta_{i}}} \right)} \approx {\begin{bmatrix} \frac{\overset{\sim}{\theta}}{2} \\ 1 \end{bmatrix} \otimes {q_{\Delta}\left( {{{\hat{\omega}}_{i} - b_{\omega_{i}}},t_{\Delta_{i}}} \right)}}},} & (19) \end{matrix}$ where {tilde over (θ)} is a 3×1 error vector, yielding: {tilde over (θ)}≈2[q _(Δ){circumflex over (ω)}_(i) −b _(ω) _(i) +n _(ω) ,t _(Δ) _(i) )⊗q _(Δ) ⁻¹({circumflex over (ω)}_(i) −b _(ω) _(i) ,t _(Δ) _(i) )]_(1:3)≈2G_(ω) n _(ω)  (20), where G_(ω)is a Jacobian matrix associated with noise n_(ω), further yielding {circumflex over (θ)}≈2[q _(i+1) ⊗q _(i) ⁻¹ ⊗qΔ ⁻¹({circumflex over (ω)}_(i) −b _(ω) _(i) ,t _(Δ) _(i) )]_(1:3)  (21), and defining a cost function and a covariance of the rotational component as: e _(q)(q _(i) ,q _(i+1) ,b _(ω) _(i) )=[q _(i+1) ⊗q _(i) ⁻¹ ⊗q _(Δ) ⁻¹({circumflex over (ω)}_(i) −b _(ω) _(i) ,t _(Δ) _(i) )]_(1:3), Σ_(q)=σ_(n) _(ω) ² G _(ω) G _(ω) ^(T)   (22); defining discrete-time motion equations, cost functions and covariance for other components as: $\begin{matrix} {\mspace{79mu} {{{p_{i + 1} = {p_{i} + {v_{i}t_{\Delta_{i}}} + {R_{i}^{T}{p_{\Delta}\left( {a_{i},t_{\Delta_{i}}} \right)}}}}\mspace{20mu} {v_{i + 1} = {v_{i} + {R_{i}^{T}{v_{\Delta}\left( {a_{i},t_{\Delta_{i}}} \right)}}}}\mspace{20mu} {b_{a_{i + 1}} = {b_{a_{i}} + {w_{a}t_{\Delta_{i}}}}}\mspace{20mu} {b_{\omega_{i + 1}} = {b_{\omega_{i}} + {w_{\omega}t_{\Delta_{i}}}}}{{e_{p}\left( {q_{i},p_{i},p_{i + 1},v_{i},b_{a_{i}}} \right)} = {{R_{i}\left( {p_{i + 1} - p_{i} - {v_{i}t_{\Delta_{i}}}} \right)} - {p_{\Delta}\left( {{{\hat{a}}_{i} - b_{a_{i}}},t_{\Delta_{i}}} \right)}}}}\mspace{20mu} {{e_{v}\left( {q_{i},v_{i},v_{i + 1},v_{i},b_{a_{i}}} \right)} = {{R_{i}\left( {v_{i + 1} - v_{i}} \right)} - {v_{\Delta}\left( {{{\hat{a}}_{i} - b_{a_{i}}},t_{\Delta_{i}}} \right)}}}\mspace{20mu} {{e_{b_{a}}\left( {b_{a_{i}},b_{a_{i + 1}}} \right)} = {b_{a_{i + 1}} - b_{a_{i}}}}\mspace{20mu} {{e_{b_{\omega}}\left( {b_{\omega_{i}},b_{\omega_{i + 1}}} \right)} = {b_{\omega_{i + 1}} - b_{\omega_{i}}}}\mspace{20mu} {\Sigma_{p} = {\frac{1}{4}\sigma_{n_{a}}^{2}t_{\Delta_{i}}^{4}I}}\mspace{20mu} {\Sigma_{v} = {\sigma_{n_{a}}^{2}t_{\Delta_{i}}^{2}I}}\mspace{20mu} {\Sigma_{b_{a}} = {\sigma_{w_{a}}^{2}t_{\Delta_{i}}^{2}I}}\mspace{20mu} {{\Sigma_{b_{\omega}} = {\sigma_{w_{\omega}}^{2}t_{\Delta_{i}}^{2}I}},}}} & (25) \end{matrix}$ where $\begin{matrix} {{{p_{\Delta}\left( {a,t_{\Delta}} \right)} = {\frac{1}{2}{at}_{\Delta}^{2}}},{{{v_{\Delta}\left( {a,t_{\Delta}} \right)} = {at}_{\Delta}};}} & (24) \end{matrix}$ defining an energy function of all motion states s₁. . . s₁ in the sliding window as: $\begin{matrix} {{\underset{s_{1}\ldots \mspace{14mu} s_{l}}{\arg \; \min}{\sum\limits_{i = 1}^{l}{\sum\limits_{j \in V_{c_{i}}}{{{\pi \left( {K\left( {R_{i}\left( {X_{j} - p_{i}} \right)} \right)} \right)} - x_{ij}}}}}} + {\sum\limits_{i = 1}^{l - 1}{{e_{q}\left( {q_{i},q_{i + 1},b_{\omega_{i}}} \right)}}_{\Sigma_{q}}^{2}} + {\sum\limits_{i = 1}^{l - 1}{{{{{e_{p}\left( {q_{i},p_{i},p_{i + 1},v_{i},b_{a_{i}}} \right)}{_{\Sigma_{p}}^{2}\quad}} + {{\quad\sum\limits_{i = 1}^{l - 1}}{e_{v}\left( {q_{i},v_{i},v_{i + 1},v_{i},b_{a_{i}}} \right)} {_{\Sigma_{v}}^{2}\quad}} + {\sum\limits_{i = 1}^{l - 1}{{{e_{b_{a}}\left( {b_{a},b_{a_{i + 1}}} \right)}{_{\Sigma_{b_{a}}}^{2}{+ \sum\limits_{i = 1}^{l - 1}}}{e_{b_{\omega}}\left( {b_{\omega_{i}},b_{\omega_{i + 1}}} \right)}}}_{\Sigma_{b_{\omega}}}^{2}}},}}}} & (26) \end{matrix}$ where l is a size of the sliding window and ∥e∥_(Σ) ²=e^(T)Σ⁻¹e is a square of a Mahalanobis distance; for a case without an IMU sensor, setting â_(i)=0, obtaining one optimal angular velocity by using the feature matching to align successive frame images: $\begin{matrix} {{{\hat{\omega}}_{i} = {\underset{\omega}{argmin}\left( {{\sum\limits_{x \in \Omega}{{{{\overset{\sim}{I}}_{i}(x)} - {{\overset{\sim}{I}}_{i + 1}\left( {\pi \left( {{{KR}_{\Delta}\left( {\omega,t_{\Delta_{i}}} \right)}K^{- 1}x^{h}} \right)} \right)}}}_{\delta_{I}}} + {\sum\limits_{{({x_{i},x_{i + 1}})} \in M_{i,{i + 1}}}{\frac{1}{\delta_{X}}{{{\pi \left( {{{KR}_{\Delta}\left( {\omega,t_{\Delta_{i}}} \right)}K^{- 1}x_{i}^{h}} \right)} - x_{i + 1}}}_{2}^{2}}}} \right)}},} & (27) \end{matrix}$ where R_(Δ)(ω,t_(Δ)) is a rotation matrix of the formula (18), M_(i,i+1) is a set of feature matchings between images i and i+1, denoting a set of IMU measurement data between frames i and i+1 as {(â_(ij), {circumflex over (ω)}_(ij), t_(ij))|=1 . . . n_(i)} if there is real IMU measurement data, where â_(ij), {circumflex over (ω)}_(ij) and t_(ij) are respectively a j_(th) linear acceleration, a rotation speed and a timestamp; and using a pre-integration technique to pre-integrate the IMU measurement data, and using the following formulas to replace components in (18) and (24): $\mspace{79mu} {{q_{\Delta}\left( \left\{ \left( {{{\hat{\omega}}_{ij} - b_{\omega_{i}}},t_{ij}} \right) \right\} \right)} \approx {{q_{\Delta}\left( {{{\hat{\omega}}_{ij} - {\hat{b}}_{\omega_{i}}},t_{ij}} \right)} + {\frac{\partial q_{\Delta}}{\partial b_{\omega}}\left( {b_{\omega} - {\hat{b}}_{\omega}} \right)}}}$ ${{p_{\Delta}\left( \left\{ \left( {{{\hat{a}}_{ij} - b_{a_{i}}},{{\hat{\omega}}_{ij} - b_{\omega_{i}}},t_{ij}} \right) \right\} \right)} \approx {{p_{\Delta}\left( \left\{ \left( {{{\hat{a}}_{ij} - {\hat{b}}_{a_{i}}},{{\hat{\omega}}_{ij} - {\hat{b}}_{\omega_{i}}},t_{ij}} \right) \right\} \right)} + {\frac{\partial p_{\Delta}}{\partial b_{a}}\left( {b_{a} - {\hat{b}}_{a}} \right)} + {\frac{\partial p_{\Delta}}{\partial b_{\omega}}\left( {b_{\omega} - {\hat{b}}_{\omega}} \right)}}},{{v_{\Delta}\left( \left\{ \left( {{{\hat{a}}_{ij} - b_{a_{i}}},{{\hat{\omega}}_{ij} - b_{\omega_{i}}},t_{ij}} \right) \right\} \right)} \approx {{v_{\Delta}\left( \left\{ \left( {{{\hat{a}}_{ij} - {\hat{b}}_{a_{i}}},{{\hat{\omega}}_{ij} - {\hat{b}}_{\omega_{i}}},t_{ij}} \right) \right\} \right)} + {\frac{\partial v_{\Delta}}{\partial b_{a}}\left( {b_{a} - {\hat{b}}_{a}} \right)} + {\frac{\partial v_{\Delta}}{\partial b_{\omega}}\left( {b_{\omega} - {\hat{b}}_{\omega}} \right)}}}$ where {circumflex over (b)}_(a) _(i) and {circumflex over (b)}_(ω) _(i) are states of b_(a) _(i) , and b_(ω) _(i) when being pre-integrated.
 6. The method for simultaneous localization and mapping according to claim 4, wherein the global optimization by the background thread comprises: for each three-dimensional point in the new key frame F_(k), calculating a maximum ray angle with respect to the existing key frame using the formula (8) first; marking the point as a fully constrained three-dimensional point if max_(i) α(i,k)≥δ_(α) and three-dimensional coordinates are successfully triangulated; then extending the existing three-dimensional plane visible in the frame F_(k) by the new and fully constrained three-dimensional point X belonging to the plane; defining a neighborhood relationship by performing Delaunay triangulation on the two-dimensional feature in the key frame F_(k); when a three-dimensional point satisfies the following three conditions at the same time, adding this point to a set V_(P) of the plane P: 1) X is not assigned to any plane, 2) X is connected with another point in V_(P), and 3) |n^(T)X+d|<δ_(P), where d_(k) is the average depth of the key frame F_(k); adopting a RANSAC algorithm to extract a new three-dimensional plane; randomly sampling three connected points in each iteration to initialize a set V_(P) of three-dimensional interior points; adopting SVD to recover a three-dimensional plane parameter P, and then starting an inner loop which iteratively adjusts extension and optimization of the plane; for each internal iteration, checking a distance from the three-dimensional points X, which are connected to the points in the set V_(P), to the plane; if |n^(T)X+d|<δ_(P), adding X to V_(P), optimizing the plane by a minimization formula for all points in V_(P): $\begin{matrix} {{\underset{P}{argmin}{\sum\limits_{X \in V_{P}}\left( {{n^{T}X} + d} \right)^{2}}},} & (28) \end{matrix}$ and iteratively extending V_(P) by repeating the above steps; and when |V_(P)|<30, discarding P and V_(P); sorting the extracted planes in a descending order according to a number of relevant three-dimensional points, and denoting a set of the sorted interior points as {V_(P) _(i) |i=1 . . . N_(P)}; starting from a first one, for any V_(P) _(i) , if a three-dimensional point X∈V_(P), also exists in a previous set of plane points V_(P) _(j) (j<i), removing this point from V_(P) _(i) ; and optimizing the three-dimensional plane parameter by adding a point-to-plane constraint in the global bundle adjustment, $\begin{matrix} {{\underset{C,X,P}{\arg \; \min}{\sum\limits_{i = 1}^{N_{F}}{\sum\limits_{j \in V_{i}}{{{\pi \left( {K\left( {R_{i}\left( {X_{j} - p_{i}} \right)} \right)} \right)} - x_{ij}}}_{\delta_{x}}}}} + {\sum\limits_{i = 1}^{N_{P}}{\sum\limits_{j \in V_{P_{i}}}{{{{n_{i}^{T}X_{j}} + d_{j}}}_{\delta_{P}}.}}}} & (29) \end{matrix}$
 7. The method for simultaneous localization and mapping according to claim 1, further comprising an initialization step of placing a virtual object on a specific plane of a real scene in a real size: initializing the tracking using a planar marker of a known size, providing a world coordinate system at a center of the planar marker, adopting n three-dimensional points X_(i), i=1, . . . , n uniformly along an edge of the planar marker, and recovering an orientation (R,t) of a camera in an initial frame by solving the following optimization equation: $\begin{matrix} {{{\underset{R,t}{\arg \; \min}{\sum\limits_{i = 1}^{n}{{{I\left( {\pi \left( {{RX}_{i} + t} \right)} \right)} - {I\left( {\pi \left( {{R\left( {X + {\delta \; X_{i}^{}}} \right)} + t} \right)} \right)}}}^{2}}} + {{{I\left( {\pi \left( {{RX}_{i} + t} \right)} \right)} - {I\left( {\pi \left( {{R\left( {X + {\delta X}_{i}^{\bot}} \right)} +} \right)} \right)} + 255}}^{2}},} & (30) \end{matrix}$ where directions of δ_(∥) and δX_(⊥) are respectively a direction along a tangent at X_(i) of the edge and a vertically inward direction at X_(i), and lengths thereof ensure that distances between projection points of X+δX_(i) ^(∥) and X+δX_(i) ^(⊥)on the image and a projection point of X_(i) are one pixel; constructing a 4-layer pyramid for the image by adopting a coarse-to-fine strategy to enlarge a convergence range, starting from a top layer, and using an optimized result as an initial value of a lower layer optimization. 